{"id":370,"date":"2017-10-29T18:33:29","date_gmt":"2017-10-29T18:33:29","guid":{"rendered":"https:\/\/peterklemperer.com\/blog\/?p=370"},"modified":"2017-10-29T20:08:05","modified_gmt":"2017-10-29T20:08:05","slug":"opencv-charuco-board-pose-estimation","status":"publish","type":"post","link":"https:\/\/peterklemperer.com\/blog\/2017\/10\/29\/opencv-charuco-board-pose-estimation\/","title":{"rendered":"OpenCV Charuco-Board Pose Estimation"},"content":{"rendered":"

The OpenCV Charuco-board pose estimation tools can provide localization estimation relative to your camera. The red-green-and blue axes drawn at the upper-right of the charuco-board taped to my wall indicates the pose estimate.<\/p>\n

\"\"<\/p>\n

These instructions are not particularly user-friendly, but got me started. I’m not really sure that my distance coefficients are set up correctly. Hopefully I’ll have a chance to clean that up and post an update if I make improvements.<\/p>\n

Please comment if you’ve got ideas for improvement or if you found this post useful.<\/p>\n

<\/p>\n

Overview<\/strong><\/p>\n

    \n
  1. Obtain accurate configuration settings<\/li>\n
  2. Compile the pose estimation program<\/li>\n
  3. Run and verify configuration settings<\/li>\n<\/ol>\n

    Step 1: Obtain accurate configuration settings<\/strong><\/p>\n

    For my program to work, you should have a board configuration file called See my previous post on opencv charuco calibration<\/a> for details.<\/p>\n

    Step 2: Compile the included pose estimation program<\/strong><\/p>\n

    Download the following python3 program and save it into the same folder as the two XML files from step 1.<\/p>\n

    # charuco_pose.py\r\n#\r\n# Peter F. Klemperer\r\n# October 29, 2017\r\nimport pdb\r\nimport time\r\nimport numpy as np\r\nimport cv2\r\nimport glob\r\n\r\nimport cv2.aruco as aruco\r\n\r\ncap = cv2.VideoCapture(0)\r\n\r\ndef read_node_real( reader, name ):\r\n node = reader.getNode( name )\r\n return node.real()\r\n\r\ndef read_node_string( reader, name ):\r\n node = reader.getNode( name )\r\n return node.string()\r\n\r\ndef read_node_matrix( reader, name ):\r\n node = reader.getNode( name )\r\n return node.mat()\r\n\r\n# read defaultConfig.xml presented to\r\n# opencv_interactive-calibration\r\nconfig_reader = cv2.FileStorage()\r\nconfig_reader.open(\"defaultConfig.xml\",cv2.FileStorage_READ)\r\n\r\naruco_parameters = aruco.DetectorParameters_create()\r\naruco_dict_num = int(read_node_real( config_reader, \"charuco_dict\" ) )\r\naruco_dict = aruco.Dictionary_get(aruco_dict_num)\r\ncharuco_square_length = int(read_node_real( config_reader, \"charuco_square_lenght\" ))\r\ncharuco_marker_size = int(read_node_real( config_reader, \"charuco_marker_size\" ))\r\nconfig_reader.release()\r\n\r\n# TODO clean this up and use config parameters?\r\n# look up definition o charuco_square_lenght, charuco_marker_size\r\n# 5 was width (x) passed to interactive-calibration\r\n# 7 was height (y) passed to interactive-calibration\r\n# .034 is square_length (meters)\r\n# .02 is marker_size (meters)\r\ncharuco_board = aruco.CharucoBoard_create( 5, 7, 0.034, 0.02, aruco_dict )\r\n\r\n# read the cameraParameters.xml file generated by\r\n# opencv_interactive-calibration\r\ncamera_reader = cv2.FileStorage()\r\ncamera_reader.open(\"cameraParameters.xml\",cv2.FileStorage_READ)\r\ncamera_matrix = read_node_matrix( camera_reader, \"cameraMatrix\" )\r\ndist_coeffs = read_node_matrix( camera_reader, \"dist_coeffs\" )\r\ncamera_reader.release()\r\n\r\n\r\nwhile(True):\r\n time.sleep( 0.1 )\r\n # Read frame from Camera\r\n # convert frame to grayscale\r\n ret, frame = cap.read()\r\n gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)\r\n\r\n\r\n\r\ncorners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, \r\n parameters=aruco_parameters)\r\n\r\n# if enough markers were detected\r\n # then process the board\r\n if( ids is not None ):\r\n #gray = aruco.drawDetectedMarkers(gray, corners)\r\n ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco(corners, ids, gray, charuco_board )\r\n \r\n # if there are enough corners to get a reasonable result\r\n if( ret > 5 ):\r\n aruco.drawDetectedCornersCharuco(frame,ch_corners,ch_ids,(0,0,255) )\r\n retval, rvec, tvec = aruco.estimatePoseCharucoBoard( ch_corners, ch_ids, charuco_board, camera_matrix, dist_coeffs )\r\n\r\n# if a pose could be estimated\r\n if( retval ) :\r\n frame = aruco.drawAxis(frame,camera_matrix,dist_coeffs,rvec,tvec,0.032)\r\n\r\n# imshow and waitKey are required for the window\r\n # to open on a mac.\r\n cv2.imshow('frame', frame)\r\n\r\nif( cv2.waitKey(1) & 0xFF == ord('q') ):\r\n break\r\n\r\ncap.release()\r\ncv2.destroyAllWindows()<\/pre>\n
    Step 3: Run and Verify<\/strong><\/p>\n
    Run the program and verify. I made the axis length the same as the black square edge to try to confirm I’d chosen the distance coefficients accurately, but I haven’t done any testing beyond that.<\/p>\n
    Summary<\/strong><\/p>\n
    There are many of these tutorials on the web, and this one is mine. I hope that you find my sharing it useful.<\/p>\n
    Resources<\/strong><\/p>\n