def triangulate(self, area=None, mode='pzq27eQ'):
"""
Perform an initial triangulation.
@param area is a max area constraint
@param mode a string of TRIANGLE switches. Refer to the TRIANGLE doc for more info about mode:
http://www.cs.cmu.edu/~quake/triangle.switch.html
@note invoke this after setting the boundary points, segments, and optionally hole positions.
"""
if not self.has_points and not self.has_segmts:
print('%s: Error. Must set points, segments, and optionally holes prior to calling "triangulate"' \
% (__file__))
return
# mode string
# z: start indexing with zero
# q<angle>: quality mesh
# e: edge
# p: planar straight line graph
# Q: quiet mode
self.mode = mode
if area:
self.area = area
mode += 'a%f' % area
if len(self.hndls) <= 1: self.hndls.append(triangulate.new())
triangulate.triangulate(mode, self.hndls[0], self.hndls[1], self.h_vor)
self.has_trgltd = True
def triangulate(self, area=None, mode='pzq27eQ'):
"""
Perform an initial triangulation. Invoke this after setting
the boundary points, segments, and optionally hole positions.
Here, area is a max area constraint and mode a string of TRIANGLE
switches. Check the TRIANGLE doc for more info about mode:
http://www.cs.cmu.edu/~quake/triangle.switch.html
"""
if not self.has_points and not self.has_segmts:
print('%s: Error. Must set points, segments, and optionally holes prior to calling "triangulate"' \
% (__file__))
return
# mode string
# z: start indexing with zero
# q<angle>: quality mesh
# e: edge
# p: planar straight line graph
# Q: quiet mode
self.mode = mode
if area:
self.area = area
mode += 'a%f'% area
if len(self.hndls) <= 1: self.hndls.append( triangulate.new() )
triangulate.triangulate(mode, self.hndls[0], self.hndls[1], self.h_vor)
self.has_trgltd = True
def triangle_graph(regions: List[Polygon], graph: Graph) -> List[Triangle]:
"""
Triangulate regions, connecting into a tree (acyclic digraph) for lookups from triangles to
the original polygon tiling.
:param regions: list of regions to be triangulated
:param graph: graph to be populated with edge from the original polygons to the triangles in their triangulations.
:return: list of all triangles in all regions
"""
logging.debug("Triangulating subdivision of %d regions" % len(regions))
triangles = []
for i, region in enumerate(regions):
logging.debug("Triangulating region %d" % i)
logging.getLogger().handlers[0].flush()
graph.add_node(region, original=True)
if isinstance(region, Triangle):
triangles.append(region)
elif region.n == 3:
region.__class__ = Triangle
triangles.append(region)
else:
# triangulation = region.triangulate()
triangulation = triangulate(region)
for triangle in triangulation:
graph.add_node(triangle, original=False)
graph.add_edge(triangle, region)
triangles.append(triangle)
return triangles
def TestMesh():
tri = trig.TriTrap()
tris = trig.triangulate(tri, 20, 21)
tris = trig.cull_triangles(tris, point_in_triangle_checker(tris))
mesh = mc.mesh_creator(tris)
# plot(mesh)
return mesh
def main(file_name, option):
global file_name_old
file_name_old = file_name
# if there is an error, try to catch it in the exception block
try:
vertices = open_file.get_vertices(file_name_old)
enumerated_vertices = []
for cordinates in vertices:
new_cordinate = cordinates.split(",")
for i in range(0, len(new_cordinate)):
new_cordinate[i] = float(new_cordinate[i])
enumerated_vertices.append(new_cordinate)
except Exception as error:
print error
print "Please try a different input"
optimization_triangulation.triangulate(enumerated_vertices, option)
def next_layer(regions: List[Triangle], boundary: Triangle, digraph: DiGraph) -> List[Triangle]:
"""
Compute the next layer in the data structure by removing O(n) points, retriangulating,
connecting new triangles in DAG to triangles with which they might overlap. We don't
compute actual intersections to save time.
:param regions: the current layer in the algorithm
:param boundary: the bounding triangle (so that the boundary is never removed)
:param digraph: digraph search tree for later location
:return: list of triangles in the next layer
"""
# Since a tiling is represented as list of polygons, points may
# appear multiple times in the tiling. We produce a mapping to
# bring all information together
point2regions = defaultdict(set)
for i, region in enumerate(regions):
for point in region.pts:
point2regions[point].add(i)
# Graph on the vertices of the tiling
graph = Graph()
for region in regions:
for u, v in zip(region.pts, np.roll(region.pts, 1)):
graph.add_edge(u, v)
# Find independent set to remove constant fraction of triangles
ind_set = planar_independent_set(graph, black_list=boundary.pts)
# Find the affected regions to be joined together, triangulate, and connect into DAG
unaffected = set(range(len(regions)))
new_regions = list()
for point in ind_set:
# Remove point and join triangles.
affected_ixs = point2regions[point]
unaffected.difference_update(affected_ixs)
affected = [regions[i] for i in affected_ixs]
new_poly = remove_point_triangulation(affected, point)
# Retriangulate
# new_triangles = new_poly.triangulate()
new_triangles = triangulate(new_poly)
new_regions += new_triangles
# Connect into DAG for lookups
for tri in new_triangles:
for ix in affected_ixs:
digraph.add_node(tri, original=False)
digraph.add_edge(tri, regions[ix])
new_regions += [regions[i] for i in unaffected]
return new_regions
def refine(self, area_ratio=2.0):
"""
Apply a refinement to the triangulation. Should be called after
performing an initial triangulation. Here, area_ratio represents
the max triangle area reduction factor.
"""
if not self.has_trgltd:
print('%s: Error. Must triangulate prior to calling "refine"' \
% (__file__))
return
self.hndls.append(triangulate.new())
mode = self.mode + 'cr'
if self.area:
self.area /= area_ratio
mode += 'a%f' % self.area
triangulate.triangulate(mode, self.hndls[-2], self.hndls[-1],
self.h_vor)
def refine(self, area_ratio=2.0):
"""
Apply a refinement to the triangulation. Should be called after
performing an initial triangulation. Here, area_ratio represents
the max triangle area reduction factor.
"""
if not self.has_trgltd:
print('%s: Error. Must triangulate prior to calling "refine"' \
% (__file__))
return
self.hndls.append( triangulate.new() )
mode = self.mode + 'cr'
if self.area:
self.area /= area_ratio
mode += 'a%f' % self.area
triangulate.triangulate(mode, self.hndls[-2],
self.hndls[-1], self.h_vor)
def getCoordinates(MAC):
global x
global y
global coordinates
SignalPower = enterMAC(MAC, IP_first)
Signal1_raw = float(SignalPower)
SignalPower = enterMAC(MAC, IP_second)
Signal2_raw = float(SignalPower)
SignalPower = enterMAC(MAC, IP_third)
Signal3_raw = float(SignalPower)
# SignalPower = enterMAC(MAC, IP_fourth)
# Signal4_raw = float(SignalPower)
Signal1, Signal2, Signal3 = [x if x > 0 else 0.01 for x in (Signal1_raw, Signal2_raw, Signal3_raw)]
coordinates = triangulate([(340.0, 70.0, Signal1), (50.0, 100.0, Signal2), (250.0, 200.0, Signal3) ]) #this config depends on actual dimensions of where sensors are placed
x=coordinates[0] + randint(-4,4)
y=coordinates[1] + randint(-4,4)
def main():
nv = 20
shape = [400, 400]
# make points
points = []
for i in range(nv + 3):
points.append(Point(random.randint(0, shape[0]), random.randint(0, shape[1])))
# sort
points = sorted(points, key=lambda x: x.x)
triangles = triangulate(points)
# triangles = []
# 画图
draw_img(shape, points, triangles)
pass
def getCoordinates(MAC):
SignalPower = enterMAC(MAC, IP_first)
Signal1_raw = float(SignalPower)
SignalPower = enterMAC(MAC, IP_second)
Signal2_raw = float(SignalPower)
SignalPower = enterMAC(MAC, IP_third)
Signal3_raw = float(SignalPower)
SignalPower = enterMAC(MAC, IP_fourth)
Signal4_raw = float(SignalPower)
Signal1, Signal2, Signal3, Signal4 = [x if x > 0 else 0.01 for x in (Signal1_raw, Signal2_raw, Signal3_raw, Signal4_raw)]
coordinates = triangulate([(0.0, 140.0, Signal1), (250.0, 40.0, Signal2), (50.0, 100.0, Signal3), (50.0, 50.0, Signal4) ]) #this config depends on actual dimensions of where sensors are placed
global coordinates
global x
global y
x=coordinates[0]
y=coordinates[1]
def wrap_triangle(poly: Polygon) -> Tuple[Triangle, List[Triangle]]:
"""
Finds a large triangle that surrounds the polygon, and tiles the gap
between the triangle the polygon.
:param poly: polygon to be surrounded
:return: tuple of the bounding triangle and the triangles in the gap
"""
"""Wraps the polygon in a triangle and triangulates the gap"""
bounding_triangle = Triangle.enclosing_triangle(poly)
# We need to expand the triangle a little bit so that the polygon side isn't on top of
# one of the triangle sides (otherwise Triangle library segfaults).
bounding_triangle = bounding_triangle.scale(1.1)
# bounding_region = bounding_triangle.triangulate(hole=poly)
bounding_region = triangulate(bounding_triangle, hole=poly)
return bounding_triangle, bounding_region
import random
from triangulate import triangulate
from draw import drawTriangles
max_x = 2000
max_y = 2000
nodes = 50
graph = []
for node in range(0, nodes):
x = random.randrange(0, max_x)
y = random.randrange(0, max_y)
graph.append((x, y))
enclosing_triangle = {(-2000, -100), (4000, -100), (1000, 4000)}
triangles = triangulate(graph, enclosing_triangle)
drawTriangles(triangles)
def visual_odometry(setup):
"""
Runs full VO pipeline, visualizes the resulting motion estimate and
returns the pose change (start-to-end).
Parameters:
-----------
setup - VO setup object, all required details for the VO algorithm.
Returns:
--------
Tba - 4x4 np.array, homogeneous pose matrix, change in robot pose
from 'after' frame to 'before' frame.
"""
matchDir = setup.projectRoot + setup.matchDir
trackDir = setup.projectRoot + setup.trackDir
ransacDir = setup.projectRoot + setup.ransacDir
# Set up stereo camera matrices.
Kl = setup.Kl
Kr = setup.Kr
Tl = np.eye(4)
Tr = np.eye(4)
Tr[0, 3] = setup.baseline
Tba = np.eye(4)
trans = np.zeros((3, 1))
# Set up figure windows, if we want them.
fig_imagept = None
fig_traject = None
if setup.plotPoints:
fig_imagept = plt.figure()
if setup.plotTrajectory:
fig_traject = plt.figure()
# Main loop, estimating incremental motion.
for i in range(setup.startInd, setup.stopInd + 1):
# Report progress.
print("At frame %d of %d." % (i, setup.stopInd))
# Load points file.
try:
points = \
loadmat("%s/%s%04d.mat" % (matchDir, setup.interPrefix, i))
ransac = \
loadmat("%s/%s%04d.mat" % (ransacDir, setup.ransacPrefix, i))
# Inlying matches / stereo points pairs.
idxs = np.array(ransac["inliers"])
points = np.array(points["common"])[idxs - 1, :]
except:
# No such file, so skip it and move on to the next one.
continue
# Typically a RANSAC loop would run here, but we have preprocessed the
# data in advance.
# Build covariance matrices - these are matrices of image plane variances
# derived from the epipolar matching function. They provide information on
# the quality of the match.
Sil = np.empty((2, 2, points.shape[0]))
Sfl = np.empty((2, 2, points.shape[0]))
for j in range(points.shape[0]):
Sil[:, :, j] = np.array([[points[j, 5], 0], [0, points[j, 6]]])
Sfl[:, :, j] = np.array([[points[j, 11], 0], [0, points[j, 12]]])
Sir = Sil.copy()
Sfr = Sfl.copy()
# Triangulate points before and after - then use the triangulated points
# for motion estimation. Note that the array Sb is 3x3xn, and contains a
# 3x3 covariance matrix for each landmark point in Pb.
Pi = np.empty((3, points.shape[0]))
Pf = np.empty((3, points.shape[0]))
Si = np.empty((3, 3, points.shape[0]))
Sf = np.empty((3, 3, points.shape[0]))
for j in range(points.shape[0]):
# Initial
_, _, Pi[:, [j]], Si[:, :, j] = \
triangulate(Kl, Kr, Tl, Tr,
points[j, 1:3].reshape(2, 1),
points[j, 3:5].reshape(2, 1),
Sil[:, :, j], Sir[:, :, j])
# Final
_, _, Pf[:, [j]], Sf[:, :, j] = \
triangulate(Kl, Kr, Tl, Tr,
points[j, 7:9].reshape(2, 1),
points[j, 9:11].reshape(2, 1),
Sfl[:, :, j], Sfr[:, :, j])
#print(Pi[:, j])
#print(Pf[:, j])
# Estimate incremental motion.
Tfi = estimate_motion_ils(Pi, Pf, Si, Sf, 100)
Tba = Tba@Tfi
#print(Tfi)
#print()
#print(Tba)
#print()
# Append new increment to path for plotting.
trans = np.append(trans, Tba[0:3, 3].reshape((3, 1)), axis = 1)
# Show trajectory.
if setup.plotTrajectory:
plt.figure(fig_traject.number)
plt.clf()
ax = fig_traject.add_subplot(111, projection = '3d')
ax.plot(trans[0, :], trans[1, :], trans[2, :], 'b-')
#ax.set_aspect("equal")
plt.grid()
plt.pause(0.01)
# Overlay motion on image.
if setup.plotPoints:
I = plt.imread("%s/l-rect-%08d_track.ppm" % (trackDir, i))
plt.figure(fig_imagept.number)
plt.clf()
plt.imshow(I)
# Overlay tracks on image.
for k in range(points.shape[0]):
plt.plot([points[k, 1], points[k, 7]], \
[points[k, 2], points[k, 8]] , c = 'b')
plt.scatter(points[:, 1], points[:, 2], s = 20, c = 'r', marker = 'x')
plt.scatter(points[:, 7], points[:, 8], s = 20, c = 'g', marker = 'o')
plt.pause(0.01)
return Tba
def triangulate(self, i):
track = self.tracks[i]
Rs = [ self.Rs[j] for j in track.camera_ids ]
ts = [ self.ts[j] for j in track.camera_ids ]
return triangulate.triangulate(self.K, Rs, ts, track.measurements)
def showPreview(self):
self.triangles, self.pointsA, self.pointsB, self.hull, constraints = triangulate.triangulate( self.pairsLayer(), self.pairsLayerRestrictToSelection(),self.constraintsLayer(), self.constraintsLayerRestrictToSelection(), self.bufferValue() )
self.updatePreview()
def doRun(self):
self._abort = False
if self.debug:
args = ['gdalinfo',
'--version',
]
sucess, result = self.runCommand(args, 'get GDAL version')
self.progress.emit("Starting RasterBender", float(0))
#####################################
# Step 1 : create the delaunay mesh #
#####################################
self.progress.emit( "Loading delaunay mesh...", float(0) )
# Create the delaunay triangulation
triangles, pointsA, pointsB, hull, constraints = triangulate.triangulate( self.pairsLayer, self.pairsLimitToSelection, self.constraintsLayer, self.constraintsLimitToSelection, self.bufferValue )
###############################
# Step 2. Opening the dataset #
###############################
self.progress.emit( "Opening the dataset... This shouldn't be too long...", float(0) )
#Open the dataset
osgeo.gdal.UseExceptions()
# Read the source data into numpy arrays
dsSource = osgeo.gdal.Open( self.sourcePath, osgeo.gdal.GA_ReadOnly )
# Get the transformation
pixW = float(dsSource.RasterXSize-1) #width in pixel
pixH = float(dsSource.RasterYSize-1) #width in pixel
rezX = dsSource.GetGeoTransform()[1] #horizontal resolution
rezY = dsSource.GetGeoTransform()[5] #vertical resolution # this should give -1 if the raster has no geotransform, but it does not...
mapW = float(dsSource.RasterXSize)*rezX #width in map units
mapH = float(dsSource.RasterYSize)*rezY #width in map units
offX = dsSource.GetGeoTransform()[0] #offset in map units
offY = dsSource.GetGeoTransform()[3] #offset in map units
self.log('pixW:{} pixH:{} rezX:{} rezY:{} mapW:{} mapH:{} offX:{} offY:{}'.format(pixW,pixH,rezX,rezY,mapW,mapH,offX,offY), True )
dsSource = None # close the source
# We copy the origin to the destination raster
# Every succequent drawing will happen on this raster, so that areas that don't move are already ok.
# We get the informations of the source layer to use the same format for output
args = ['gdalinfo',
# '-json', # -json doesn't exist in GDAL<2.0
self.sourcePath,
]
sucess, result = self.runCommand(args, 'get the file infos')
# output_format = json.loads(result)['driverShortName'] # -json doesn't exist in GDAL<2.0, so we use this:
if not sucess: return
output_format = None
geotransform_found = False
for line in result.split('\n'):
if line[0:8]=='Driver: ':
output_format = line[8:].split('/')[0]
if line[0:13]=='Pixel Size = ':
geotransform_found=True
# And we create a copy
args = ['gdal_translate',
self.sourcePath,
self.targetPath,
]
# If we has an input format, we set it using -of parameter
if output_format:
self.log('Output format was found : {}'.format(output_format), True)
args.extend(['-of',output_format])
else:
self.log('Output format was not found.', True)
if geotransform_found:
self.log('Geotransform was found.', True)
else:
# If we have no geotransform, we use GCPs to match 1 pixel = 1 map unit.
args.extend(['-gcp',0,0,0,0])
args.extend(['-gcp',0,1,0,-1])
args.extend(['-gcp',1,0,1,0])
rezY = -rezY # hack, see above
self.log('Geotransform was not found. We created GCPs', True)
sucess, result = self.runCommand(args, 'copy the file')
if not sucess: return
def qgsPointToXY(qgspoint):
"""
Returns a point in pixels coordinates given a point in map coordinates
"""
return ( (qgspoint.x() - offX) / rezX + 1.0 , (qgspoint.y() - offY) / rezY + 1.0 )
# We loop through every triangle to create a GDAL affine transformation
count = len(triangles)
for i,triangle in enumerate(triangles):
if self._abort:
self.error.emit( "Aborted on triangle %i out of %i..." % (i+1, count))
return
self.progress.emit( "Computing triangle %i out of %i..." % (i+1, count), float(i)/float(count) )
# aX are the pixels points of the initial triangles
a0 = qgsPointToXY(pointsA[triangle[0]])
a1 = qgsPointToXY(pointsA[triangle[1]])
a2 = qgsPointToXY(pointsA[triangle[2]])
# bx are the map points of the destination triangle
b0 = pointsB[triangle[0]]
b1 = pointsB[triangle[1]]
b2 = pointsB[triangle[2]]
# Step 1 : we do an affine transformation by providing 3 -gcp points
# here we compute the parameters for srcwin, so that we don't compute the transformation on the whole raster
# we have a 2 pixels margins, hence the +/- 2 and the enclosing max/min (to avoid overbound)
xMin = min(a0[0],a1[0],a2[0])
yMin = min(a0[1],a1[1],a2[1])
xMax = max(a0[0],a1[0],a2[0])
yMax = max(a0[1],a1[1],a2[1])
xoff = xMin-2
yoff = yMin-2
xsize = xMax-xMin+4
ysize = yMax-yMin+4
tempTranslated = QTemporaryFile()
if self.debug: tempTranslated.setAutoRemove(False)
tempTranslated.open()
args = ['gdal_translate',
'-gcp', a0[0]-xoff,a0[1]-yoff,b0[0],b0[1],
'-gcp', a1[0]-xoff,a1[1]-yoff,b1[0],b1[1],
'-gcp', a2[0]-xoff,a2[1]-yoff,b2[0],b2[1],
'-srcwin', xoff, yoff, xsize, ysize,
self.sourcePath,
tempTranslated.fileName(),
]
sucess, result = self.runCommand(args, 'create the temporaray file %i out of %i' % (i+1, count))
if not sucess: return
# Step 2 : we draw the transformed layer on the target layer by providing a cutline (corresponding to the destination triangle)
# We create a vector polygon to feed into GDAL's -cutline argument
clip = QgsGeometry.fromPolygon([[b0,b1,b2,b0]]).buffer(.5*abs(rezX)+.5*abs(rezY),2)
# Since it must be a GDAL format, we have to create a .csv file (hah, command line tools...)
tempWKT = QTemporaryFile( os.path.join(QDir.tempPath(),'XXXXXX.csv') )
if self.debug: tempWKT.setAutoRemove(False)
tempWKT.open()
content = 'WKT\tID\n"%s"\t1' % (clip.exportToWkt())
tempWKT.write(content)
tempWKT.close()
args = [ 'gdalwarp',
'-cutline', tempWKT.fileName(),
'-cblend', '1',
'-dstnodata', '-999',
'-r', self.samplingMethod,
tempTranslated.fileName(),
self.targetPath,
]
sucess, result = self.runCommand(args, 'patch the triangle %i out of %i' % (i+1, count))
if not sucess: return
self.finished.emit()
return
# Camera poses (left, right).
Twl = np.eye(4)
Twl[:3, :3] = dcm_from_rpy([-np.pi/2, 0, 0]) # Tilt for visualization.
Twr = Twl.copy()
Twr[0, 3] = 0.4 # Baseline.
# Image plane points (left, right).
pl = np.array([[241], [237.0]])
pr = np.array([[230], [238.5]])
# Image plane uncertainties (covariances).
Sl = np.eye(2)
Sr = np.eye(2)
[Pl, Pr, P, S] = triangulate(Kl, Kr, Twl, Twr, pl, pr, Sl, Sr)
# Visualize...
fig = plt.figure()
ax = fig.add_subplot(111, projection = '3d')
ax.plot(np.array([Twl[0, 3], Pl[0, 0]]),
np.array([Twl[1, 3], Pl[1, 0]]),
np.array([Twl[2, 3], Pl[2, 0]]), 'b-')
ax.plot(np.array([Twr[0, 3], Pr[0, 0]]),
np.array([Twr[1, 3], Pr[1, 0]]),
np.array([Twr[2, 3], Pr[2, 0]]), 'r-')
ax.plot(np.array([Pl[0, 0], Pr[0, 0]]),
np.array([Pl[1, 0], Pr[1, 0]]),
np.array([Pl[2, 0], Pr[2, 0]]), 'g-')
ax.plot([P[0, 0]], [P[1, 0]], [P[2, 0]], 'bx', markersize = 8)
try:
fields = line.split()
dists = np.array(map(lambda x: int(x, 16) / 1000.0, fields[2:5]))
for i in range(len(dists)):
if dists[i] == 0:
dist_circles[i].set_fill('r')
else:
dist_circles[i].set_fill(None)
except ValueError:
continue
if any(dists == 0):
print("Skipping because of zero: ", dists)
plt.pause(0.01)
continue
results = triangulate(dists, points, position[-1, :])
X = results.x
X = [np.clip(X[0], 1.0, 33.0), np.clip(X[1], 1.0, 15.0)]
zone = find_area(X, polygons)
color_area(X, polygons, coloredareas)
loc_error = np.abs(results.fun).mean()
# print(results)
position[:-1, :] = position[1:, :]
position[-1, :] = X
print("{:<.02f}".format(time()), "position:", X, "from distance:",
dists)
mean_pos = position.mean(axis=0)
stderr = 2 * position.std(axis=0) + 0.2
ellipse.center = mean_pos
def final():
camera = libcpm.Camera()
camera.setup()
# cpp matcher:
pmatcher = libcpm.PatchMatch()
pmatcher.init(camera, 20)
# python matcher:
#bgs0, bgs1 = [], []
#for k in range(20):
#m1 = camera.get_for_py(0)
#m1 = np.array(m1, copy=True)
#m2 = camera.get_for_py(1)
#m2 = np.array(m2, copy=True)
#bgs0.append(m1)
#bgs1.append(m2)
#matcher = Matcher(BackgroundSegmentor(bgs0), BackgroundSegmentor(bgs1))
runner = get_parallel_runner('../data/cpm.npy')
viewer = libcpm.StereoCameraViewer(camera)
viewer.start()
C1, C0, d1, d0 = load_camera_from_calibr(
'../calibr-1211/camchain-homeyihuaDesktopCPM3D_kalibrfinal3.yaml')
queue = deque(maxlen=2)
ctx = zmq.Context()
sok = ctx.socket(zmq.PUSH)
global args
sok.connect('tcp://{}:8888'.format(args.host))
def cpp_matcher(m1, m2, o1, o2):
o1 = libcpm.Mat(o1)
o2 = libcpm.Mat(o2)
out = pmatcher.match_with_hm(m1, m2, o1, o2)
return np.asarray(out).reshape(14, 4) #14 x 2image x (x,y)
pts3ds = []
cnt = 0
while True:
cnt += 1
print 'begin---', time.time()
m1 = camera.get_for_py(0)
m1r = np.array(m1, copy=False)
m2 = camera.get_for_py(1)
m2r = np.array(m2, copy=False)
m1s = cv2.resize(m1r, (368, 368))
m2s = cv2.resize(m2r, (368, 368))
print 'after resize---', time.time()
o1, o2 = runner(m1s, m2s)
print 'after cpm---', time.time()
#pts14x4 = matcher.match(m1r, m2r, o1, o2)
pts14x4 = cpp_matcher(m1, m2, o1, o2)
#to_save = (m1s, m2s, o1, o2, pts14x4)
#fout = open('full-recording/{:04d}.dat'.format(cnt), 'wb')
#fout.write(dumps(to_save))
#fout.close()
print 'after match---', time.time()
queue.append(pts14x4)
p2d = np.mean(queue, axis=0)
p3ds = np.zeros((14, 3))
for c in range(14):
p3d = triangulate(C0, C1, p2d[c, :2], p2d[c, 2:])
p3ds[c, :] = p3d
sok.send(dumps(p3ds))
print p3ds
print 'after send---', time.time()
print '-----------------'
def main():
#command line argument
parser = argparse.ArgumentParser()
parser.add_argument("-p",
"--plot",
default="no_img",
help="Either no_img, or plot_img")
args = parser.parse_args()
PLOT = args.plot
# load images, which are stored in a folder named 'rectified' in the same directory as this file
data_dir = os.path.dirname(os.path.abspath(__file__))
ic = io.collection.ImageCollection(data_dir + '/rectified/*.png')
half_idx = len(ic) // 2
#load a few frames for testing
'''
left_ic = ic[0+513:500+513]
right_ic = ic[half_idx+513:half_idx+500+513]
ic = None
'''
left_ic = ic[:half_idx]
right_ic = ic[half_idx:]
# images have shape (480, 640, 3)
csv_file = open("points.csv", mode='w')
csv_features_file = open("features.csv", mode='w')
csv_writer = csv.writer(csv_file,
delimiter=',',
quotechar='"',
quoting=csv.QUOTE_MINIMAL)
csv_feature_writer = csv.writer(csv_features_file,
delimiter=',',
quotechar='"',
quoting=csv.QUOTE_MINIMAL)
# initial camera pose
C = np.diag(np.ones(4))
# store the last valid frames in case of skipping frames
prevl = []
prevr = []
pts = np.array([])
for i in range(len(left_ic) - 1):
print("Iteration ", i, "/", len(left_ic) - 1)
# current frame
imgl1 = left_ic[i]
imgr1 = right_ic[i]
imgl2 = left_ic[i + 1]
imgr2 = right_ic[i + 1]
if PLOT.lower() == 'no_img':
matchesl1, matchesr1, matchesl2, matchesr2 = match_features(
imgl1, imgr1, imgl2, imgr2)
if PLOT.lower() == 'plot_img':
matchesl1, matchesr1, matchesl2, matchesr2 = match_features_plot(
imgl1, imgr1, imgl2, imgr2)
if (matchesl1.shape[0] >= 3):
# get matches
F, inliers_a1, inliers_b1, inliers_a2, inliers_b2, = ransac_F_Matrix(
matchesl1, matchesr1, matchesl2, matchesr2)
std_threshold = 2
# triangulate to find real-world points
coords3d1 = triangulate(inliers_a1, inliers_b1)
coords3d2 = triangulate(inliers_a2, inliers_b2)
if (len(coords3d1) == 0 or len(coords3d2) == 0):
print("skipping frame \n")
continue
z1 = coords3d1[:, 2]
z2 = coords3d2[:, 2]
if (i == 0):
pts = np.append(pts, z1)
pts = np.append(pts, z2)
if (len(pts) > 100):
pts = pts[:100]
mean = np.mean(pts)
std = np.std(pts)
mean1 = np.mean(z1)
mean2 = np.mean(z2)
zstd1 = mean1 / std
zstd2 = mean2 / std
print("std:", zstd1, zstd2)
# both frames k and k+1 invalid, skip
if (zstd1 >= std_threshold and zstd2 >= std_threshold):
print("skipping frame \n")
continue
# frame k invalid, frame k+1 valid
elif (zstd1 >= std_threshold and zstd2 < std_threshold):
# use last valid frame if exists, otherwise skip
if (prevl != [] and prevr != []):
matchesl1, matchesr1, matchesl2, matchesr2 = match_features(
prevl, prevr, imgl2, imgr2)
F, inliers_a1, inliers_b1, inliers_a2, inliers_b2, = ransac_F_Matrix(
matchesl1, matchesr1, matchesl2, matchesr2)
coords3d1 = triangulate(inliers_a1, inliers_b1)
coords3d2 = triangulate(inliers_a2, inliers_b2)
else:
print("skipping frame \n")
continue
# frame k valid, frame k+1 invalid
elif (zstd1 < std_threshold and zstd2 >= std_threshold):
prevl = copy.deepcopy(imgl1)
prevr = copy.deepcopy(imgr1)
print("skipping frame \n")
continue
# both frames valid
else:
prevl = []
prevr = []
inliers = coords3d1.shape[0]
coords_abs = C.T @ np.append(
coords3d1, np.ones((inliers, 1)), axis=1).T
csv_feature_writer.writerows(coords_abs[0:3, :].T)
C_new = update_camera_pose(coords3d1, coords3d2, C)
pose_distance = np.linalg.norm(C_new[0:3, 3] - C[0:3, 3])
rejection_threshold = 0.5 #meters
print("Pose", C[0:3, 3])
print("Pose Distance", pose_distance)
if (pose_distance < rejection_threshold):
C = C_new
else:
print("pose rejected")
plot_C[i] = C[0:3, 3].T
csv_writer.writerow(plot_C[i])
print("")
gif(plot_C)
def reconstruct(scandir='../scans_undistort/manny/grab_0_u/', thresh=0.015):
def _intersect_matlab(a, b):
a1, ia = np.unique(a, return_index=True)
b1, ib = np.unique(b, return_index=True)
aux = np.concatenate((a1, b1))
aux.sort()
c = aux[:-1][aux[1:] == aux[:-1]]
return c, ia[np.isin(a1, c)], ib[np.isin(b1, c)]
def _find_index_good(goodpixels):
assert (np.shape(goodpixels) == (H, W))
# return a 1D index array of goodpixels
ret = [[], []]
for i in range(H):
for j in range(W):
if goodpixels[i][j]:
ret[0].append(j)
ret[1].append(i)
return np.array(ret)
# read calibration data saved from last calibration run
with open("../cache/C0_CALIB.pkl", "rb") as c0: # right
R_mat, R_rvec, R_tvec, R_dist = pickle.load(c0)
with open("../cache/C1_CALIB.pkl", "rb") as c1: # left
L_mat, L_rvec, L_tvec, L_dist = pickle.load(c1)
# set calibration data selection index
SELECT = 2
######################################################
# start reconstruction
R_h, R_h_good = dc.decode(scandir + 'frame_C0_', 0, 19, thresh)
R_v, R_v_good = dc.decode(scandir + 'frame_C0_', 20, 39, thresh)
L_h, L_h_good = dc.decode(scandir + 'frame_C1_', 0, 19, thresh)
L_v, L_v_good = dc.decode(scandir + 'frame_C1_', 20, 39, thresh)
# save image size info
assert (np.shape(R_h) == np.shape(L_v))
H, W = np.shape(R_h)
# combine horizontal and vertical by bit shift + and operation
L_h_shifted = np.left_shift(L_h.astype(int), 10)
R_h_shifted = np.left_shift(R_h.astype(int), 10)
L_C = np.bitwise_or(L_h_shifted, L_v.astype(int))
R_C = np.bitwise_or(R_h_shifted, R_v.astype(int))
L_good = np.logical_and(L_v_good, L_h_good)
R_good = np.logical_and(R_v_good, R_h_good)
# now perform background substraction
R_color = dc.im2double(
cv2.imread(scandir + 'color_C0_01.png', cv2.IMREAD_COLOR))
R_background = dc.im2double(
cv2.imread(scandir + 'color_C0_00.png', cv2.IMREAD_COLOR))
L_color = dc.im2double(
cv2.imread(scandir + 'color_C1_01.png', cv2.IMREAD_COLOR))
L_background = dc.im2double(
cv2.imread(scandir + 'color_C1_00.png', cv2.IMREAD_COLOR))
R_colormap = abs(R_color - R_background)**2 > thresh
L_colormap = abs(L_color - L_background)**2 > thresh
R_ok = np.logical_or(R_colormap[:, :, 0], R_colormap[:, :, 1])
R_ok = np.logical_or(R_colormap[:, :, 2], R_ok)
L_ok = np.logical_or(L_colormap[:, :, 0], L_colormap[:, :, 1])
L_ok = np.logical_or(L_colormap[:, :, 2], L_ok)
R_good = np.logical_and(R_ok, R_good)
L_good = np.logical_and(L_ok, L_good)
fig, (ax1, ax2) = plt.subplots(ncols=2, figsize=(15, 20))
ax1.imshow(L_C * L_good, cmap='jet')
ax1.set_title('Left')
ax2.imshow(R_C * R_good, cmap='jet')
ax2.set_title('Right')
plt.show()
# find coordinates of pixels that were successfully decoded
# R_coord, L_coord in 1D indices
R_coord = _find_index_good(R_good)
L_coord = _find_index_good(L_good)
# pull out CODE values at successful pixels
# (notice this is a little bit different from MATLAB)
R_C_good = R_C[R_good]
L_C_good = L_C[L_good]
# perform intersection
matched, iR, iL = _intersect_matlab(R_C_good, L_C_good)
# get pixel coordinates of pixels matched
# change R_coord, L_coord to 2D first
xR = R_coord[:, iR]
xL = L_coord[:, iL]
# Now, triangulate the matched pixels using the first calibration result
camL = (L_mat, L_rvec[SELECT], L_tvec[SELECT])
camR = (R_mat, R_rvec[SELECT], R_tvec[SELECT])
X = tr.triangulate(xL, xR, camL, camR)
# Display triangulation result
fig = plt.figure()
ax = Axes3D(fig)
ax.scatter(X[0, :], X[1, :], X[2, :])
ax.view_init(45, 0)
ax.set_xlabel('x axis')
ax.set_ylabel('y axis')
ax.set_zlabel('z axis')
plt.show()
# save to MATLAB file for easier 3D viewing
import scipy.io
scipy.io.savemat('../cache/reconstructed.mat', mdict={'X': X})
# return reconstruction result for meshing
return [X, xL, xR, L_color, R_color]
K1 = intrinsics['K1']
K2 = intrinsics['K2']
K1 = np.transpose(K1)
K2 = np.transpose(K2)
E = essentialMatrix(F, K1, K2)
#Find Camera Projection Matrix
Rt1 = np.concatenate((np.identity(3), np.zeros(
(3, 1))), axis=1) #Assuming not rotation or translation in first image
P1 = np.matmul(K1, Rt1)
Rt2s = camera2(E)
P2s = np.zeros(Rt2s.shape)
Xs = np.zeros((x1.shape[0], 4, 4)) #x y z
depth_cnt = np.zeros((4))
dets = np.zeros((4))
for i in range(4):
Rt2s[:, 3, i] = Rt2s[:, 3, i] / Rt2s[2, 3, i]
P2s[:, :, i] = np.matmul(K2, Rt2s[:, :, i])
Xs[:, :, i] = triangulate(P1, x1, P2s[:, :, i], x2)
depth_cnt[i] = np.sum(np.greater(Xs[:, 2, i], 0)) #looks just at Z
dets[i] = np.linalg.det(Rt2s[:, 0:3, i])
correct_P2 = np.argmax(depth_cnt)
#correct_P2 = 1
X = Xs[:, :, correct_P2]
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(X[:, 0], X[:, 1], X[:, 2], c='r', marker='o')
#plt.scatter(X[:,1],X[:,0])
plt.show()
error = reProjError(P1, x1, P2s[:, :, correct_P2], x2, X)
def run(self):
self._abort = False
self.progress.emit("Starting RasterBender", float(0), float(0))
#####################################
# Step 1 : create the delaunay mesh #
#####################################
self.progress.emit( "Loading delaunay mesh...", float(0), float(0) )
# Create the delaunay triangulation
triangles, pointsA, pointsB, hull, constraints = triangulate.triangulate( self.pairsLayer, self.pairsLimitToSelection, self.constraintsLayer, self.constraintsLimitToSelection, self.bufferValue )
###############################
# Step 2. Opening the dataset #
###############################
self.progress.emit( "Opening the dataset... This shouldn't be too long...", float(0), float(0) )
#Open the dataset
gdal.UseExceptions()
# Read the source data into numpy arrays
dsSource = gdal.Open( self.sourcePath, gdal.GA_ReadOnly )
sourceDataR = gdalnumeric.BandReadAsArray(dsSource.GetRasterBand(1))
sourceDataG = gdalnumeric.BandReadAsArray(dsSource.GetRasterBand(2))
sourceDataB = gdalnumeric.BandReadAsArray(dsSource.GetRasterBand(3))
# Get the transformation
pixW = float(dsSource.RasterXSize-1) #width in pixel
pixH = float(dsSource.RasterYSize-1) #width in pixel
mapW = float(dsSource.RasterXSize)*dsSource.GetGeoTransform()[1] #width in map units
mapH = float(dsSource.RasterYSize)*dsSource.GetGeoTransform()[5] #width in map units
offX = dsSource.GetGeoTransform()[0] #offset in map units
offY = dsSource.GetGeoTransform()[3] #offset in map units
# Open the target into numpy array
#dsTarget = gdal.Open(self.targetPath, gdal.GA_Update )
driver = gdal.GetDriverByName( "GTiff" )
dsTarget = driver.CreateCopy( self.targetPath, dsSource, 0 )
#dsTarget.SetGeoTransform( dsSource.GetGeoTransform() )
dsTarget = None #close
def xyToQgsPoint(x, y):
return QgsPoint( offX + mapW * (x/pixW), offY + mapH * (y/pixH) )
def qgsPointToXY(qgspoint):
return ( int((qgspoint.x() - offX) / mapW * pixW ) , int((qgspoint.y() - offY) / mapH * pixH ) )
#######################################
# Step 3A. Looping through the blocks #
#######################################
#Loop through every block
blockCountX = dsSource.RasterXSize//self.blockSize+1
blockCountY = dsSource.RasterYSize//self.blockSize+1
blockCount = blockCountX*blockCountY
blockI = 0
displayTotal = dsSource.RasterXSize*dsSource.RasterYSize
displayStep = min((self.blockSize**2)/20,10000) # update gui every n steps
self.progress.emit( "Starting computation... This can take a while..." , float(0), float(0))
for blockNumY in range(0, blockCountY ):
blockOffsetY = blockNumY*self.blockSize
blockH = min( self.blockSize, dsSource.RasterYSize-blockOffsetY )
if blockH <= 0: continue
for blockNumX in range(0, blockCountX ):
blockOffsetX = blockNumX*self.blockSize
blockW = min( self.blockSize, dsSource.RasterXSize-blockOffsetX )
if blockW <= 0: continue
blockI += 1
pixelCount = blockW*blockH
pixelI = 0
blockRectangle = QgsRectangle( xyToQgsPoint(blockOffsetX, blockOffsetY),
xyToQgsPoint(blockOffsetX+blockW, blockOffsetY+blockH) ) # this is the shape of the block, used for optimization
# We check if the block intersects the hull, if not, we skip it
if not hull.intersects( blockRectangle ):
self.progress.emit( "Block %i out of %i is out of the convex hull, we skip it..." % (blockI, blockCount), float(0), float(blockI/float(blockCount) ) )
continue
# We create the trifinder for the block
blockTrifinder = trifinder.Trifinder( pointsB, triangles, blockRectangle )
targetDataR = gdalnumeric.BandReadAsArray(dsSource.GetRasterBand(1),blockOffsetX,blockOffsetY,blockW,blockH)
targetDataG = gdalnumeric.BandReadAsArray(dsSource.GetRasterBand(2),blockOffsetX,blockOffsetY,blockW,blockH)
targetDataB = gdalnumeric.BandReadAsArray(dsSource.GetRasterBand(3),blockOffsetX,blockOffsetY,blockW,blockH)
#######################################
# Step 3B. Looping through the pixels #
#######################################
# Loop through every pixel
for y in range(0, blockH):
for x in range(0, blockW):
# If abort was called, we finish the process
if self._abort:
self.error.emit( "Aborted on pixel %i out of %i on block %i out of %i..." % (pixelI, pixelCount, blockI, blockCount ), float(0), float(0))
return
pixelI+=1
# Ever now and then, we update the status
if pixelI%displayStep == 0:
self.progress.emit("Working on pixel %i out of %i on block %i out of %i... Trifinder has %i triangles" % (pixelI, pixelCount, blockI, blockCount,len(blockTrifinder.triangles) ), float(pixelI)/float(pixelCount),float(blockI)/float(blockCount) )
# We find in which triangle the point lies using the trifinder.
p = xyToQgsPoint(blockOffsetX+x, blockOffsetY+y)
tri = blockTrifinder.find( p )
if tri is None:
# If it's in no triangle, we don't change it
continue
# If it's in a triangle, we transform the coordinates
newP = trimapper.map( p,
pointsB[tri[0]], pointsB[tri[1]], pointsB[tri[2]],
pointsA[tri[0]], pointsA[tri[1]], pointsA[tri[2]] )
newX, newY = qgsPointToXY( newP )
# TODO : this would maybe get interpolated results
#ident = sourceRaster.dataProvider().identify( pt, QgsRaster.IdentifyFormatValue)
#targetDataR[y][x] = ident.results()[1]
#targetDataG[y][x] = ident.results()[2]
#targetDataB[y][x] = ident.results()[3]
try:
if newY<0 or newX<0: raise IndexError() #avoid looping
targetDataR[y][x] = sourceDataR[newY][newX]
targetDataG[y][x] = sourceDataG[newY][newX]
targetDataB[y][x] = sourceDataB[newY][newX]
except IndexError, e:
targetDataR[y][x] = 0
targetDataG[y][x] = 0
targetDataB[y][x] = 0
# Write to the image
dsTarget = gdal.Open(self.targetPath, gdal.GA_Update )
gdalnumeric.BandWriteArray(dsTarget.GetRasterBand(1), targetDataR, blockOffsetX, blockOffsetY)
gdalnumeric.BandWriteArray(dsTarget.GetRasterBand(2), targetDataG, blockOffsetX, blockOffsetY)
gdalnumeric.BandWriteArray(dsTarget.GetRasterBand(3), targetDataB, blockOffsetX, blockOffsetY)
dsTarget = None
def triangulate_marker_points(frameL, frameR, HistInfo, camCalFile,
sterCalFile, outL, outR, template_L,
template_points_L, template_R, template_points_R,
frameCount, prevMarkerParams, trialName):
# will draw detections on copy of frameL and frameR
outImL = frameL.copy()
outImR = frameR.copy()
prevFrame = frameCount > 0
# extract features, match from L to R
for histList in HistInfo:
# get the marker detection info
hist_em = histList[0]
# number of modes in GMM
N = histList[1]
# select channel
N_select = histList[2]
# key describing markers, "marker" or "tool"
key = histList[3]
if (prevFrame):
# extract prev params
prevParamsL = prevMarkerParams[0]
prevParamsR = prevMarkerParams[1]
else:
prevParamsL = []
prevParamsR = []
# key specific stuff
if key == "background":
# min gray level value
val_Thresh = 60
probImLAll = computeMarkerProb(frameL, hist_em, N, val_Thresh)
probImRAll = computeMarkerProb(frameR, hist_em, N, val_Thresh)
# channel we want for marker detection
probImL = probImLAll[N_select]
probImR = probImRAll[N_select]
# if first frame save the left and right background probability images
if (frameCount == 0):
cv2.imwrite("left_template.jpg", probImL)
cv2.imwrite("right_template.jpg", probImR)
# find template in image
# match marker template to probability image, always listed in same order so don't need to match
matchesL, bestParamsL = markerMatch(probImL, template_L,
template_points_L, prevFrame,
prevParamsL)
matchesR, bestParamsR = markerMatch(probImR, template_R,
template_points_R, prevFrame,
prevParamsR)
# drawing color
color = (255, 255, 0)
# triangulate 3D background fiducial points
marker_points = triangulate(camCalFile, sterCalFile, matchesL,
matchesR)
# save image detection points
marker_matches = [matchesL, matchesR]
elif key == "tool":
# find marker matches between the two frames
matchesL, matchesR, Kp_L, Kp_R = blobMatch(frameL, frameR,
histList, frameCount)
# drawing color
color = (0, 255, 0)
# triangulate 3D tool fiducial points
tool_points = triangulate(camCalFile, sterCalFile, matchesL,
matchesR)
# draw detections on image
for ptL, ptR in zip(matchesL, matchesR):
outImL = cv2.circle(outImL, (int(ptL[0]), int(ptL[1])), 5, color,
-1)
outImR = cv2.circle(outImR, (int(ptR[0]), int(ptR[1])), 5, color,
-1)
# outIm = cv2.drawKeypoints(outIm, Kp_L, np.array([]), color, cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
outL.write(outImL)
outR.write(outImR)
# return triangulated marker points
return marker_points.transpose(), tool_points.transpose(), [
bestParamsL, bestParamsR
], marker_matches
def run(self):
self._abort = False
self.progress.emit("Starting RasterBender", float(0), float(0))
#####################################
# Step 1 : create the delaunay mesh #
#####################################
self.progress.emit("Loading delaunay mesh...", float(0), float(0))
# Create the delaunay triangulation
triangles, pointsA, pointsB, hull, constraints = triangulate.triangulate(
self.pairsLayer, self.pairsLimitToSelection, self.constraintsLayer,
self.constraintsLimitToSelection, self.bufferValue)
###############################
# Step 2. Opening the dataset #
###############################
self.progress.emit(
"Opening the dataset... This shouldn't be too long...", float(0),
float(0))
#Open the dataset
gdal.UseExceptions()
# Read the source data into numpy arrays
dsSource = gdal.Open(self.sourcePath, gdal.GA_ReadOnly)
sourceDataR = gdalnumeric.BandReadAsArray(dsSource.GetRasterBand(1))
sourceDataG = gdalnumeric.BandReadAsArray(dsSource.GetRasterBand(2))
sourceDataB = gdalnumeric.BandReadAsArray(dsSource.GetRasterBand(3))
# Get the transformation
pixW = float(dsSource.RasterXSize - 1) #width in pixel
pixH = float(dsSource.RasterYSize - 1) #width in pixel
mapW = float(dsSource.RasterXSize) * dsSource.GetGeoTransform()[
1] #width in map units
mapH = float(dsSource.RasterYSize) * dsSource.GetGeoTransform()[
5] #width in map units
offX = dsSource.GetGeoTransform()[0] #offset in map units
offY = dsSource.GetGeoTransform()[3] #offset in map units
# Open the target into numpy array
#dsTarget = gdal.Open(self.targetPath, gdal.GA_Update )
driver = gdal.GetDriverByName("GTiff")
dsTarget = driver.CreateCopy(self.targetPath, dsSource, 0)
#dsTarget.SetGeoTransform( dsSource.GetGeoTransform() )
dsTarget = None #close
def xyToQgsPoint(x, y):
return QgsPoint(offX + mapW * (x / pixW), offY + mapH * (y / pixH))
def qgsPointToXY(qgspoint):
return (int((qgspoint.x() - offX) / mapW * pixW),
int((qgspoint.y() - offY) / mapH * pixH))
#######################################
# Step 3A. Looping through the blocks #
#######################################
#Loop through every block
blockCountX = dsSource.RasterXSize // self.blockSize + 1
blockCountY = dsSource.RasterYSize // self.blockSize + 1
blockCount = blockCountX * blockCountY
blockI = 0
displayTotal = dsSource.RasterXSize * dsSource.RasterYSize
displayStep = min((self.blockSize**2) / 20,
10000) # update gui every n steps
self.progress.emit("Starting computation... This can take a while...",
float(0), float(0))
for blockNumY in range(0, blockCountY):
blockOffsetY = blockNumY * self.blockSize
blockH = min(self.blockSize, dsSource.RasterYSize - blockOffsetY)
if blockH <= 0: continue
for blockNumX in range(0, blockCountX):
blockOffsetX = blockNumX * self.blockSize
blockW = min(self.blockSize,
dsSource.RasterXSize - blockOffsetX)
if blockW <= 0: continue
blockI += 1
pixelCount = blockW * blockH
pixelI = 0
blockRectangle = QgsRectangle(
xyToQgsPoint(blockOffsetX, blockOffsetY),
xyToQgsPoint(blockOffsetX + blockW, blockOffsetY + blockH)
) # this is the shape of the block, used for optimization
# We check if the block intersects the hull, if not, we skip it
if not hull.intersects(blockRectangle):
self.progress.emit(
"Block %i out of %i is out of the convex hull, we skip it..."
% (blockI, blockCount), float(0),
float(blockI / float(blockCount)))
continue
# We create the trifinder for the block
blockTrifinder = trifinder.Trifinder(pointsB, triangles,
blockRectangle)
targetDataR = gdalnumeric.BandReadAsArray(
dsSource.GetRasterBand(1), blockOffsetX, blockOffsetY,
blockW, blockH)
targetDataG = gdalnumeric.BandReadAsArray(
dsSource.GetRasterBand(2), blockOffsetX, blockOffsetY,
blockW, blockH)
targetDataB = gdalnumeric.BandReadAsArray(
dsSource.GetRasterBand(3), blockOffsetX, blockOffsetY,
blockW, blockH)
#######################################
# Step 3B. Looping through the pixels #
#######################################
# Loop through every pixel
for y in range(0, blockH):
for x in range(0, blockW):
# If abort was called, we finish the process
if self._abort:
self.error.emit(
"Aborted on pixel %i out of %i on block %i out of %i..."
% (pixelI, pixelCount, blockI, blockCount),
float(0), float(0))
return
pixelI += 1
# Ever now and then, we update the status
if pixelI % displayStep == 0:
self.progress.emit(
"Working on pixel %i out of %i on block %i out of %i... Trifinder has %i triangles"
% (pixelI, pixelCount, blockI, blockCount,
len(blockTrifinder.triangles)),
float(pixelI) / float(pixelCount),
float(blockI) / float(blockCount))
# We find in which triangle the point lies using the trifinder.
p = xyToQgsPoint(blockOffsetX + x, blockOffsetY + y)
tri = blockTrifinder.find(p)
if tri is None:
# If it's in no triangle, we don't change it
continue
# If it's in a triangle, we transform the coordinates
newP = trimapper.map(p, pointsB[tri[0]],
pointsB[tri[1]], pointsB[tri[2]],
pointsA[tri[0]], pointsA[tri[1]],
pointsA[tri[2]])
newX, newY = qgsPointToXY(newP)
# TODO : this would maybe get interpolated results
#ident = sourceRaster.dataProvider().identify( pt, QgsRaster.IdentifyFormatValue)
#targetDataR[y][x] = ident.results()[1]
#targetDataG[y][x] = ident.results()[2]
#targetDataB[y][x] = ident.results()[3]
try:
if newY < 0 or newX < 0:
raise IndexError() #avoid looping
targetDataR[y][x] = sourceDataR[newY][newX]
targetDataG[y][x] = sourceDataG[newY][newX]
targetDataB[y][x] = sourceDataB[newY][newX]
except IndexError, e:
targetDataR[y][x] = 0
targetDataG[y][x] = 0
targetDataB[y][x] = 0
# Write to the image
dsTarget = gdal.Open(self.targetPath, gdal.GA_Update)
gdalnumeric.BandWriteArray(dsTarget.GetRasterBand(1),
targetDataR, blockOffsetX,
blockOffsetY)
gdalnumeric.BandWriteArray(dsTarget.GetRasterBand(2),
targetDataG, blockOffsetX,
blockOffsetY)
gdalnumeric.BandWriteArray(dsTarget.GetRasterBand(3),
targetDataB, blockOffsetX,
blockOffsetY)
dsTarget = None