Welcome to JetCobot repository
4.AR vision
AR vision
1. Overview
Augmented Reality, referred to as "AR", is a technology that cleverly integrates virtual information with the real world. It widely uses multimedia, three-dimensional modeling, real-time tracking and registration, intelligent interaction, sensing and other technical means to simulate computer-generated text, images, three-dimensional models, music, video and other virtual information and apply them to the real world. The two types of information complement each other, thus achieving "enhancement" of the real world.
The AR system has three outstanding characteristics: ① Information integration of the real world and the virtual world; ② Real-time interactivity; ③ Adding and positioning virtual objects in three-dimensional space.
Augmented reality technology includes new technologies and new means such as multimedia, three-dimensional modeling, real-time video display and control, multi-sensor fusion, real-time tracking and registration, and scene fusion.
2. How to use
When using the AR case, the camera's internal parameters are required, otherwise it will not work (the factory image has completed the camera internal parameter calibration). The internal parameter file is in the same directory as the code.
2.1 Camera intrinsic calibration
Start the monocular camera
roslaunch usb_cam usb_cam-test.launch
Start the calibration node
xxxxxxxxxxrosrun camera_calibration cameracalibrator.py image:=/usb_cam/image_raw --size 9x6 --square 0.02
size: the number of internal corner points of the calibrated chessboard, for example, 9X6, the corner points have six rows and nine columns.
square: the side length of the chessboard, unit: meters.
image: the image topic published by the camera
Calibration interface
X: The left and right movement of the chessboard in the camera's field of view
Y: The up and down movement of the chessboard in the camera's field of view
Size: The forward and backward movement of the chessboard in the camera's field of view
Skew: The tilt and rotation of the chessboard in the camera's field of view
After successful startup, place the chessboard in the center of the screen and change different positions.
The system will recognize it automatically. The best situation is that the lines under [X], [Y], [Size], and [Skew] will change from red to yellow and then to green as data is collected, and fill as much as possible.
- Click [CALIBRATE] to calculate the camera internal parameters. The more pictures you have, the longer it will take. Just wait. (Sixty or seventy pictures is enough. Too many will cause the camera to get stuck.)
- Click [SAVE] to save the calibration results to [/tmp/calibrationdata.tar.gz] of the current terminal.
After the calibration is completed, you can move out the [/tmp/calibrationdata.tar.gz] file to check the contents.
xxxxxxxxxxsudo mv /tmp/calibrationdata.tar.gz ~
After decompression, there are the calibrated images, an ost.txt file and an ost.yaml file. ost.yaml is the internal parameters for calibrating the camera.
Copy the internal parameters here and overwrite them to astra.yaml in the ~/jetcobot_ws/src/jetcobot_visual/AR.
Note: The factory image has been calibrated with parameters and can be used directly by starting the program. If the camera screen is started, please close the camera process first and then start the program.
2.2 Run program
Input following command:
xxxxxxxxxxroscorerosrun jetcobot_visual simple_AR.py
After the program starts, place the chessboard in front of the camera. You need to see the entire chessboard, otherwise the program will exit.
After the entire chessboard is recognized, the following 12 effects will be displayed.
xxxxxxxxxx["Triangle", "Rectangle", "Parallelogram","WindMill","TableTennisTable", "Ball","Arrow", "Knife", "Desk","Bench", "Stickman", "ParallelBars"]
Click on the image and press F to toggle the displayed effects.
2.3 RANSAC plan
Algorithm principle: Use RANSAC scheme to find object pose from 3D-2D point correspondence
RanSaC algorithm (Random Sampling Consistency) was originally a classic algorithm for data processing. Its function is to extract specific components in an object under a lot of noise. The following figure is an illustration of the effect of RanSaC algorithm. Some points in the figure clearly satisfy a certain line, and another group of points is pure noise. The purpose is to find the equation of the line under a lot of noise. At this time, the amount of noise data is 3 times that of the line.
xxxxxxxxxxIf the least squares method is used, this effect cannot be achieved and the straight line will be slightly above the straight line in the figure.
The basic assumptions of RANSAC are:
The data consists of "inliers", i.e., the distribution of the data can be explained by some model parameters;
"Outliers" are data that cannot fit the model;
Data other than this is noise.
Outliers can be caused by: extreme values of noise; incorrect measurement methods; incorrect assumptions about the data. RANSAC also makes the following assumptions: given a set of (usually small) inliers, there is a process that can estimate the model parameters; and the model can explain or fit the inliers.
2.4 About code
Code path: ~/jetcobot_ws/src/jetcobot_visual/AR/simple_AR.py
x
#!/usr/bin/env python# -*- coding: utf-8 -*-import osimport sysimport timeimport rospyimport rospkgimport cv2 as cvimport numpy as npfrom cv_bridge import CvBridgefrom std_msgs.msg import Stringfrom sensor_msgs.msg import CompressedImage,Image
class simple_AR: def __init__(self): rospy.on_shutdown(self.cancel) self.index = 0 self.frame = None self.img_name = 'img' self.patternSize = (6, 9) self.bridge = CvBridge() self.flip = rospy.get_param("~flip", False) # 加载相机内参矩阵、畸变系数 # Load the camera internal parameter matrix and distortion coefficient yaml_path = rospkg.RosPack().get_path("jetcobot_visual") + '/AR/astra.yaml' if os.path.exists(yaml_path): fs = cv.FileStorage(yaml_path, cv.FileStorage_READ) self.cameraMatrix = fs.getNode("camera_matrix").mat() self.distCoeffs = fs.getNode("distortion_coefficients").mat() else: self.distCoeffs, self.cameraMatrix = (), () self.objectPoints = np.zeros((6 * 9, 3), np.float32) self.objectPoints[:, :2] = np.mgrid[0:6, 0:9].T.reshape(-1, 2) self.graphics = ["Triangle", "Rectangle", "Parallelogram","WindMill", "TableTennisTable", "Ball", "Arrow", "Knife", "Desk", "Bench", "Stickman", "ParallelBars"] self.Graphics = self.graphics[self.index] self.axis = np.float32([ [0, 0, -1], [0, 8, -1], [5, 8, -1], [5, 0, -1], [1, 2, -1], [1, 6, -1], [4, 2, -1], [4, 6, -1], [1, 0, -4], [1, 8, -4], [4, 0, -4], [4, 8, -4], [1, 2, -4], [1, 6, -4], [4, 2, -4], [4, 6, -4], [0, 1, -4], [3, 2, -1], [2, 2, -3], [3, 2, -3], [1, 2, -3], [2, 2, -4], [2, 2, -5], [0, 4, -4], [2, 3, -4], [1, 3, -4], [4, 3, -5], [4, 5, -5], [1, 2, -3], [1, 6, -3], [5, 2, -3], [5, 6, -3], [3, 4, -5], [0, 6, -4], [5, 6, -4], [2, 8, -4], [3, 8, -4], [2, 6, -4], [2, 0, -4], [1, 5, -4], [3, 0, -4], [3, 2, -4], [0, 3, -4], [1, 2, -4], [4, 2, -4], [5, 3, -4], [2, 7, -4], [3, 7, -4], [3, 3, -1], [3, 5, -1], [1, 5, -1], [1, 3, -1], [3, 3, -3], [3, 5, -3], [1, 5, -3], [1, 3, -3], [1, 3, -6], [1, 5, -6], [3, 3, -4], [3, 5, -4], [0, 0, -4], [3, 1, -4], [1, 1, -4], [0, 2, -4], [2, 4, -4], [4, 4, -4], [0, 8, -4], [5, 8, -4], [5, 0, -4], [0, 4, -5], [5, 4, -4], [5, 4, -5], [2, 5, -1], [2, 7, -1], [2, 6, -3], [2, 6, -5], [2, 5, -3], [2, 7, -3] ]) self.sub_graphics = rospy.Subscriber('/Graphics_topic', String, self.choose_Graphics) self.pub_img = rospy.Publisher("/simpleAR/camera", Image, queue_size=1)
def cancel(self): self.sub_graphics.unregister() self.pub_img.unregister() cv.destroyAllWindows() rospy.loginfo("Shutting down this node.") def choose_Graphics(self, msg): if not isinstance(msg, String): return if msg.data in self.graphics: self.Graphics = msg.data else: self.graphics_update()
def graphics_update(self): self.index += 1 if self.index >= len(self.graphics): self.index = 0 self.Graphics = self.graphics[self.index]
def process(self, img, action): if self.flip == True: img = cv.flip(img, 1) if action == ord('f') or action == ord('F'): self.graphics_update() gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) # 查找每个图片的角点 # Find the corner of each image retval, corners = cv.findChessboardCorners( gray, self.patternSize, None, flags=cv.CALIB_CB_ADAPTIVE_THRESH + cv.CALIB_CB_NORMALIZE_IMAGE + cv.CALIB_CB_FAST_CHECK) # 查找角点亚像素 # Find corner subpixels if retval: corners = cv.cornerSubPix( gray, corners, (11, 11), (-1, -1), (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)) # 计算对象姿态solvePnPRansac # Compute object pose solvePnPRansac retval, rvec, tvec, inliers = cv.solvePnPRansac( self.objectPoints, corners, self.cameraMatrix, self.distCoeffs) # 输出图像点和雅可比矩阵 # Output image points and Jacobian matrix image_Points, jacobian = cv.projectPoints( self.axis, rvec, tvec, self.cameraMatrix, self.distCoeffs, ) img = self.draw(img, corners, image_Points) cv.putText(frame, self.Graphics, (240, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (0, 0, 255), 1) self.pub_img.publish(self.bridge.cv2_to_imgmsg(img, "bgr8")) return img
def draw(self, img, corners, image_Points): # drawContours函数中绘图颜色顺序是bgr # drawContours the color order of the drawing is BGR img_pts = np.int32(image_Points).reshape(-1, 2) if self.Graphics == "Triangle": cv.drawContours(img, [np.array([img_pts[14], img_pts[15], img_pts[23]])], -1, (255, 0, 0), -1) elif self.Graphics == "Rectangle": cv.drawContours(img, [np.array([img_pts[12], img_pts[13], img_pts[15], img_pts[14]])], -1, (0, 255, 0), -1) elif self.Graphics == "Parallelogram": cv.drawContours(img, [np.array([img_pts[12], img_pts[10], img_pts[15], img_pts[9]])], -1, (65, 105, 225), 1) elif self.Graphics == "WindMill": cv.drawContours(img, [np.array([img_pts[60], img_pts[38], img_pts[61], img_pts[21]])], -1, (0, 0, 255), -1) cv.drawContours(img, [np.array([img_pts[10], img_pts[14], img_pts[58], img_pts[21]])], -1, (0, 0, 255), -1) cv.drawContours(img, [np.array([img_pts[62], img_pts[63], img_pts[23], img_pts[21]])], -1, (0, 0, 255), -1) cv.drawContours(img, [np.array([img_pts[25], img_pts[64], img_pts[65], img_pts[21]])], -1, (0, 0, 255), -1) cv.line(img, tuple(img_pts[64]), tuple(img_pts[35]), (0, 255, 0), 3) elif self.Graphics == "TableTennisTable": cv.line(img, tuple(img_pts[0]), tuple(img_pts[60]), (255, 0, 0), 3) for i in range(1, 4): cv.line(img, tuple(img_pts[i]), tuple(img_pts[65 + i]), (255, 0, 0), 3) cv.drawContours(img, [np.array([img_pts[60], img_pts[66], img_pts[67], img_pts[68]])], -1, (0, 255, 0), -1) cv.drawContours(img, [np.array([img_pts[23], img_pts[69], img_pts[71], img_pts[70]])], -1, (0, 0, 255), -1) elif self.Graphics == "Ball": cv.circle(img, tuple(img_pts[22]), 30, (0, 0, 255), -1) elif self.Graphics == "Arrow": cv.drawContours(img, [np.array([img_pts[13], img_pts[34], img_pts[36]])], -1, (0, 255, 0), -1) cv.drawContours(img, [np.array([img_pts[37], img_pts[15], img_pts[10], img_pts[38]])], -1, (0, 255, 0), -1) elif self.Graphics == "Knife": cv.drawContours(img, [np.array([img_pts[58], img_pts[24], img_pts[35], img_pts[47]])], -1, (160, 252, 0), -1) cv.drawContours(img, [np.array([img_pts[40], img_pts[38], img_pts[21], img_pts[41]])], -1, (30, 144, 255), -1) cv.drawContours(img, [np.array([img_pts[42:46]])], -1, (0, 0, 255), -1) elif self.Graphics == "Desk": for i in range(4): cv.line(img, tuple(img_pts[4 + i]), tuple(img_pts[12 + i]), (163, 148, 128), 3) cv.drawContours(img, [np.array([img_pts[14], img_pts[12], img_pts[13], img_pts[15]])], -1, (0, 199, 140), -1) elif self.Graphics == "Bench": for i in range(4): cv.line(img, tuple(img_pts[48 + i]), tuple(img_pts[52 + i]), (255, 0, 0), 3) cv.drawContours(img, [img_pts[52:56]], -1, (0, 0, 255), -1) cv.drawContours(img, [img_pts[54:58]], -1, (139, 69, 19), -1) elif self.Graphics == "Stickman": cv.line(img, tuple(img_pts[18]), tuple(img_pts[4]), (0, 0, 255), 3) cv.line(img, tuple(img_pts[18]), tuple(img_pts[6]), (0, 0, 255), 3) cv.line(img, tuple(img_pts[18]), tuple(img_pts[21]), (0, 0, 255), 3) cv.line(img, tuple(img_pts[21]), tuple(img_pts[19]), (0, 0, 255), 3) cv.line(img, tuple(img_pts[21]), tuple(img_pts[20]), (0, 0, 255), 3) cv.line(img, tuple(img_pts[21]), tuple(img_pts[22]), (0, 0, 255), 3) cv.circle(img, tuple(img_pts[22]), 15, (0, 0, 255), -1) cv.line(img, tuple(img_pts[74]), tuple(img_pts[72]), (0, 255, 0), 3) cv.line(img, tuple(img_pts[74]), tuple(img_pts[73]), (0, 255, 0), 3) cv.line(img, tuple(img_pts[74]), tuple(img_pts[37]), (0, 255, 0), 3) cv.line(img, tuple(img_pts[37]), tuple(img_pts[76]), (0, 255, 0), 3) cv.line(img, tuple(img_pts[37]), tuple(img_pts[77]), (0, 255, 0), 3) cv.line(img, tuple(img_pts[37]), tuple(img_pts[75]), (0, 255, 0), 3) cv.circle(img, tuple(img_pts[75]), 15, (0, 255, 0), -1) elif self.Graphics == "ParallelBars": for i in range(4): cv.line(img, tuple(img_pts[4 + i]), tuple(img_pts[12 + i]), (255, 0, 0), 3) cv.line(img, tuple(img_pts[8]), tuple(img_pts[9]), (0, 0, 255), 3) cv.line(img, tuple(img_pts[10]), tuple(img_pts[11]), (0, 0, 255), 3) return img
if __name__ == '__main__': rospy.init_node("simple_AR", anonymous=False) capture = cv.VideoCapture(0) capture.set(6, cv.VideoWriter_fourcc('M', 'J', 'P', 'G')) capture.set(cv.CAP_PROP_FRAME_WIDTH, 640) capture.set(cv.CAP_PROP_FRAME_HEIGHT, 480) print("capture get FPS : ", capture.get(cv.CAP_PROP_FPS)) pTime, cTime = 0, 0 ar = simple_AR() while capture.isOpened(): ret, frame = capture.read() action = cv.waitKey(1) & 0xFF if action == ord('q') or action == ord("Q"): break frame = ar.process(frame, action) cTime = time.time() fps = 1 / (cTime - pTime) pTime = cTime text = "FPS : " + str(int(fps)) cv.putText(frame, text, (20, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (0, 0, 255), 1) if len(sys.argv) != 1: if sys.argv[1]=="true" or sys.argv[1]=="True": cv.imshow('frame', frame) else:cv.imshow('frame', frame) capture.release() cv.destroyAllWindows()The program flow chart is as follows.
