From 837f33c0bc7924f7db4217cd3f023a8ece623679 Mon Sep 17 00:00:00 2001 From: jhuai Date: Wed, 30 Oct 2024 12:43:45 +0800 Subject: [PATCH] cut holes in aprilgrid --- .../kalibr/python/kalibr_create_target_pdf | 63 ++++++++++++++++++- 1 file changed, 60 insertions(+), 3 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_create_target_pdf b/aslam_offline_calibration/kalibr/python/kalibr_create_target_pdf index 27e31dcc0..a9580561e 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_create_target_pdf +++ b/aslam_offline_calibration/kalibr/python/kalibr_create_target_pdf @@ -104,6 +104,18 @@ def generateAprilBoard(canvas, n_cols, n_rows, tagSize, tagSpacing=0.25, tagFami #create one tag numTags = n_cols*n_rows + # Calculate the bounding box + xmin = 0 + ymin = 0 + xmax = n_cols * (1+tagSpacing) * tagSize + tagSpacing * tagSize + ymax = n_rows * (1+tagSpacing) * tagSize + tagSpacing * tagSize + bbox = (xmin, ymin, xmax, ymax) + border_bbox = bbox + border_rect = path.rect(border_bbox[0], border_bbox[1], + border_bbox[2] - border_bbox[0], + border_bbox[3] - border_bbox[1]) + c.fill(border_rect, [color.rgb.white]) + #draw tags for y in range(0,n_rows): for x in range(0,n_cols): @@ -112,7 +124,6 @@ def generateAprilBoard(canvas, n_cols, n_rows, tagSize, tagSpacing=0.25, tagFami pos = ( x*(1+tagSpacing)*tagSize, y*(1+tagSpacing)*tagSize) generateAprilTag(canvas, pos, tagSize, tagSpacing, id, tagFamililyData, rotation=2) #c.text(pos[0]+0.45*tagSize, pos[1]-0.7*tagSize*tagSpacing, "{0}".format(id)) - #draw axis pos = ( -1.5*tagSpacing*tagSize, -1.5*tagSpacing*tagSize) c.stroke(path.line(pos[0], pos[1], pos[0]+tagSize*0.3, pos[1]), @@ -128,10 +139,49 @@ def generateAprilBoard(canvas, n_cols, n_rows, tagSize, tagSpacing=0.25, tagFami c.text(pos[0], pos[1]+tagSize*0.3, "y") #text - caption = "{0}x{1} tags, size={2}cm and spacing={3}cm".format(n_cols,n_rows,tagSize,tagSpacing*tagSize) + caption = "{0}x{1} tags, size={2:.2f} cm and spacing={3:.2f} cm".format(n_cols,n_rows,tagSize,tagSpacing*tagSize) c.text(pos[0]+0.6*tagSize, pos[0], caption) +def cutHoles(c, n_cols, n_rows, tagSize, holeRadius, tagSpacing, draw_reflector=False): + #convert to cm + tagSize = tagSize*100.0 + holeRadius = holeRadius*100.0 + multiplier = 1 + tagSpacing + holeGapH = tagSize * (multiplier * (n_cols - 2) + tagSpacing) - holeRadius * (2 + math.sqrt(2)) + holeGapV = tagSize * (multiplier * (n_rows - 2) + tagSpacing) - holeRadius * (2 + math.sqrt(2)) + print("holeGapH: {0:.2f} cm, holeGapV: {1:.2f} cm".format(holeGapH, holeGapV)) + + # Note the origin is at the corner between the first small black square and the first apriltag. + rh = holeRadius * math.sqrt(2) / 2 + centers = [(tagSize + rh, tagSize + rh), + (tagSize + rh, tagSize + rh + holeRadius * 2 + holeGapV), + (tagSize + rh + holeRadius * 2 + holeGapH, tagSize + rh), + (tagSize + rh + holeRadius * 2 + holeGapH, tagSize + rh + holeRadius * 2 + holeGapV)] + for pos in centers: + c.fill(path.circle(pos[0], pos[1], holeRadius), [color.rgb.white]) + + if draw_reflector: + # draw a regular triangle at the center of the board + ra = 77.6 / 10 # the height of a corner reflector in cm + rc = ra * math.sqrt(2) + halfrc = rc / 2 + rh = rc / 2 * math.sqrt(3) + rhtwothirds = rh * 2 / 3 + center = (tagSize * (multiplier * n_cols - tagSpacing) / 2, tagSize * (multiplier * n_rows - tagSpacing) / 2) + x1, y1 = center[0], center[1] + rhtwothirds + x2, y2 = center[0] - halfrc, center[1] - rh / 3 + x3, y3 = center[0] + halfrc, center[1] - rh / 3 + + # Create a path object for the triangle + triangle_path = path.path(path.moveto(x1, y1), + path.lineto(x2, y2), + path.lineto(x3, y3), + path.closepath()) + fill_color = [color.gray(0.8)] + c.fill(triangle_path, fill_color) + + def generateCheckerboard(canvas, n_cols, n_rows, size_cols, size_rows): #convert to cm size_cols = size_cols*100.0 @@ -161,6 +211,8 @@ if __name__ == "__main__": kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.08 --tspace 0.3 Example Checkerboard: kalibr_create_target_pdf --type checkerboard --nx 6 --ny 6 -csx 0.05 --csy 0.1 + Example python3 ./src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_create_target_pdf \ + --type apriltag --nx 6 --ny 6 --tsize 0.14 --tspace 0.3 --cut-holes --hole-radius=0.20 """ #setup the argument list @@ -172,6 +224,7 @@ if __name__ == "__main__": genericOptions = parser.add_argument_group('Generic grid options') genericOptions.add_argument('--type', dest='gridType', help='The grid pattern type. (\'apriltag\' or \'checkerboard\')') + genericOptions.add_argument('--cut-holes', action='store_true', dest='cutHoles', help='Cut holes in the apriltags for lidar extrinsic calibration') genericOptions.add_argument('--nx', type=int, default=6, dest='n_cols', help='The number of tags in x direction (default: %(default)s)\n') genericOptions.add_argument('--ny', type=int, default=7, dest='n_rows', help='The number of tags in y direction (default: %(default)s)') @@ -180,6 +233,7 @@ if __name__ == "__main__": aprilOptions.add_argument('--tspace', type=float, default=0.3, dest='tagspacing', help='The space between the tags in fraction of the edge size [0..1] (default: %(default)s)') aprilOptions.add_argument('--tfam', default='t36h11', dest='tagfamiliy', help='Familiy of April tags {0} (default: %(default)s)'.format(list(AprilTagCodes.TagFamilies.keys()))) aprilOptions.add_argument('--skip-ids', default='', dest='skipIds', help='Space-separated list of tag ids to leave blank (default: none)') + aprilOptions.add_argument('--hole-radius', type=float, default=0.08 * 1.3, dest='holeRadius', help=('The radius of the holes [m] (default: %(default)s)')) checkerOptions = parser.add_argument_group('Checkerboard arguments') checkerOptions.add_argument('--csx', type=float, default=0.05, dest='chessSzX', help='The size of one chessboard square in x direction [m] (default: %(default)s)') @@ -204,12 +258,15 @@ if __name__ == "__main__": #draw the board if parsed.gridType == "apriltag": generateAprilBoard(canvas, parsed.n_cols, parsed.n_rows, parsed.tsize, parsed.tagspacing, parsed.tagfamiliy, parsed.skipIds) + if parsed.cutHoles: + cutHoles(c, parsed.n_cols, parsed.n_rows, parsed.tsize, parsed.holeRadius, parsed.tagspacing) + elif parsed.gridType == "checkerboard": generateCheckerboard(c, parsed.n_cols, parsed.n_rows, parsed.chessSzX, parsed.chessSzY) else: print("[ERROR]: Unknown grid pattern") sys.exit(0) - + #write to file c.writePDFfile(parsed.output)