def init_patchs_array(self, size):
self.patchs_array = [[Patch(
(0, 0), 0, self.noised_image)] * self.denoised_image.height
] * self.denoised_image.width
# Image width
for x in range(self.denoised_image.width):
# Image height
for y in range(self.denoised_image.height):
self.patchs_array[x][y] = Patch((x, y), size,
self.noised_image)
def __init__(self, numPatches): ### Number Of Patches ### self.patches = [] self.patchDistances = [] self.numPatches = numPatches self.totalArea = 0 self.completePatches = 0 # May start right on the first mile. start = rand.randint(0, 2000) for i in range(numPatches): length = rand.randint(100, 3600) ### Total Square feet self.patches.append(Patch.Patch(start, length)) self.totalArea += self.patches[i].getArea() start += length # start has to be at least 100 feet from the previous patch. start += rand.randint(100, 2000) # numPatches - 1: N patches, N - 1 inbetween spaces for i in range(numPatches - 1): nextPatch = self.patches[i + 1] previousPatch = self.patches[i] self.patchDistances.append(nextPatch.getStart() - previousPatch.getEnd())
def initializePatches(self):
#Instantiate Patches
#Create a list to hold the patches. We first fill these with
#zeros to hold the place for each Patch object
self.patch_dict = {
i: {
j: 0
}
for i in range(self.rows) for j in range(self.cols)
}
for i in range(self.rows):
for j in range(self.cols):
#replace zeros with actual Patch objects
good = "sugar" if i + j >= self.rows else "water"
self.patch_dict[i][j] = Patch(self, i, j, self.sugarMap[i][j],
good)
self.empty_patches = RandomDict({(i, j): self.patch_dict[i][j]
for i in range(self.rows)
for j in range(self.cols)})
def __init__(self, name, width, height):
## Setup the simulator
## 1. Create World
self.world = World(name, width, height)
self.agents = list()
## 2. Fill in salinity and inundation data for each patch
## TODO: Get env data from GIS dataset
for x in range(0, width + 1):
for y in range(0, height + 1):
p = Patch(1.0 - 1 / (x + 1), 1.0, 1.0, True)
self.world.setPatch(x, y, p)
## 3. Plant some mangroves
## TODO: Get tree location from GIS dataset
## TODO: Plant different species of plants
## For now, plant trees manually
for i in range(0, min(width, height) + 1):
plant = Mangrove(0.5, 0, self.world, i, i)
self.agents.append(plant)
self.world.putAgent(i, i, plant)
self.step = 0 ## No tree death, recruitment or storms for now
def initializePatches(self):
#Instantiate Patches
#Create a dictionary to hold the patches, organize as grid.
#We first fill these with zeros as placeh holders
self.patch_dict = {
row: {
col: 0
}
for row in range(self.rows) for col in range(self.cols)
}
for row in range(self.rows):
for col in range(self.cols):
# replace zeros with actual Patch objects
good = "sugar" if row + col < self.cols else "water"
self.patch_dict[row][col] = Patch(self, row, col,
self.sugarMap[row][col],
good)
# use RandomDict - O(n) time complexity - for choosing random empty patch
self.empty_patches = RandomDict({(row, col): self.patch_dict[row][col]
for row in range(self.rows)
for col in range(self.cols)})
def extractMeasurements(t, depthMsg, boundingBoxMsg, odometryMsg, imageMsg):
global f_odometry
newMeasurements = []
cylinderR = 1
boundingBoxMinPixelSpace = 15
currPos = [
odometryMsg.pose.position.x, odometryMsg.pose.position.y,
odometryMsg.pose.position.z
]
traj3d.append(currPos)
depthImage = bridge.imgmsg_to_cv2(depthMsg, desired_encoding="32FC1")
# Rotate RGB image if necessary
depthImage = cv2.rotate(depthImage, cv2.ROTATE_180)
rgbImage = bridge.imgmsg_to_cv2(imageMsg, desired_encoding="bgr8")
patches = []
for b in boundingBoxMsg.bounding_boxes:
# Check if desired object class
if (b.Class != 'car' and b.Class != 'truck'):
continue
xmin = max(b.xmin, 0)
ymin = max(b.ymin, 0)
xmax = b.xmax
ymax = b.ymax
# Check if detection not at boundary
if (xmin < boundingBoxMinPixelSpace or ymin < boundingBoxMinPixelSpace
or xmax > imgWidth - 1 - boundingBoxMinPixelSpace
or ymax > imgHeight - 1 - boundingBoxMinPixelSpace):
continue
p = Patch(depthImage[ymin:ymax + 1, xmin:xmax + 1], xmin, xmax, ymin,
ymax)
# Only use detections with depth information inside circle
if ~np.isnan(p.medianDepthCircle):
patches.append(p)
# Sort patches according to minimum depth values inside bounding boxes
patches.sort(key=operator.attrgetter('minDepth'))
qi = [
odometryMsg.pose.orientation.w, odometryMsg.pose.orientation.x,
odometryMsg.pose.orientation.y, odometryMsg.pose.orientation.z
]
R_world_imu = quadToRot(qi)
for p in patches:
rgbImage = ImagePainter.markPatch(rgbImage, p)
pxCoords = p.getFlippedCenterCoordinates()
coord3d = Kinv.dot(pxCoords.T)
coord3d = coord3d / np.linalg.norm(coord3d) * (
p.medianDepthCircle + cylinderR
) # Measurement vector in cameara frame
coord3d = R_imu_cam.dot(coord3d) - np.array(x_imu_cam)
coord3d = R_world_imu.dot(coord3d) - np.array(x_world_imu) + np.array(
currPos)
coord3d = coord3d.tolist()
#coord3d = (np.array(currPos)+(R_world_imu.dot(R_imu_cam.dot(coord3d)))-np.array(x_world_imu)).tolist()
measurements3d.append(coord3d)
newMeasurements.append(coord3d)
f_measurements.writeMeasurement(t, coord3d)
f_odometry.writeOdometry(t, odometryMsg)
return rgbImage, patches, newMeasurements
