Actual combat | how to make a slam track truth acquisition device?

Click on the above “ Xiaobai studies vision ”, Optional plus " Star standard " or “ Roof placement ”

 Heavy dry goods , First time delivery 

The author yangxiaodong authorized the reprint of this article , Unauthorized reproduction is prohibited

original text ： https://zhuanlan.zhihu.com/p/57235987

SLAM A very important indicator of performance is Positioning accuracy , Public indoor data sets generally use Mobile capture equipment Get the true value of the device track . But mobile capture equipment Expensive ,OptiTrack Over a million , domestic Nokov >20W. No mobile capture equipment , It's hard for us to use Data collected by yourself assessment SLAM Positioning performance .
So , We built a cheap track truth capture device . Use a fixed Monocular camera obtain Plane motion Of Trace truth . We mainly use this method to measure the trajectory of indoor mobile robot .
This paper gives the principle and code .
Be careful ： It can only measure 2D Motion in a plane , No 3D.

1. principle

 

Here's the picture , One is fixed on the bracket Down looking monocular camera , It is used to capture a flat sticker on the top of the robot ArUco code .ArUco The pose of the robot is known （ This should be easy to do ）. The problem we need to solve is to use monocular camera to measure ArUco The position and pose of the code on the plane . actually ,OpenCV Provided ArUco The library of , Can achieve ArUco The identification of code , And provide code Pixel coordinates of the four corners . If we can calculate the coordinates of the four corners in the plane , The robot's position and posture on the plane can be calculated （ Position and orientation ）. Let's talk about how to calculate the position of a point on the plane .

The principle of truth capture of plane trajectory

In the world coordinate system , One 3D The point of , Project to image coordinates , Can be expressed as

T Is external parameter . What we want to measure is 2D Coordinates in the plane , For convenience , You can set the origin of the world coordinate system on the plane ,Z The axis is perpendicular to this plane （ Namely ArUco That plane ）. that Of z =0 . type （1） It can become

Points on the plane And pixels on the image It's a connection between them . Between them is a homography matrix H .

We just need to know this homography matrix H , You can use the image pixels Get its on the plane 2D coordinate

 

2. The question now is how the homography matrix comes from ？

Slave mode （2） It can be seen that , As long as you know the internal parameters of the camera K And external parameters T ,H And then there is . Parameter K To say , Everyone should be familiar with . The key is T, Here we use a plane target to calibrate . Concrete , As shown in the figure below, place a checkerboard target on ArUco Above the yard , adopt PnP The method can calculate the pose transformation between the target coordinate system and the camera coordinate system T'. Be careful , The plane of the checkerboard target is not ArUco The plane of the code , Because the checkerboard target has a thickness t. This thickness t We can easily measure , Consider to be known . that , Camera coordinate system and ArUco The coordinate transformation between the world coordinate systems on the surface is

thus , We get the external parameters T . adopt （2） Then we can get homography matrix H.

Calibration homography matrix H

3. The following problem is to calculate the pose of the robot

Pictured above , Just now we mentioned , You can get... On the image ArUco code Pixel coordinates of the four corners , It's written here as

Use （3） type , The coordinates of these four points in the world coordinate system can be calculated respectively （ Is the coordinates in the plane ）

About the position of the robot , In fact, we only need the coordinates of a point , Namely ArUco Coordinates of code center point . So we combine four points into one point ：

Is the position of the robot .

Next, we need to calculate the posture of the robot , stay 2D The plane is facing . The orientation can be calculated with two points , Now we have four dots in our hands , Then average it .θ Is the orientation of the robot .

4. The accuracy of measurement

Since you want to use it as a truth value , It requires sufficient measurement accuracy . Now let's analyze the measurement accuracy . Let's do it （3） an

elimination  λ You can get

If the image u Precision of pixel extraction in the direction - The variance of ,v Precision of direction - The variance of . That is, the covariance of the pixel is

So the measured point on the plane The covariance of is

At this point, the accuracy analysis is finished , That is to say, the homography matrix is marked H after , adopt （12） The approximate measurement accuracy can be calculated by the formula .

actually , You can also bring the internal and external parameters of the camera into （3） type , combining （12） type , An analytic precision expression can be obtained . This is more complicated . If the camera is installed only around the camera coordinate system relative to the measurement plane x If the axis rotates by an angle , The accuracy distribution in the measurement area is shown in the following figure . If the camera is facing the measuring plane , The measurement accuracy of the measurement area is the same . Of course , The higher the resolution of the camera, the higher the precision . The longer the focal length, the higher the accuracy , But at the same time, the measurable area will become smaller .

My own camera is Daheng MER-302-56U3C The camera ,2048×1536 Pixels ; The lens is kowa LM3NC1M,3.5mm wide-angle lens ; The distance between the camera and the measuring plane is about 2m. The measurable range is about 4m×3m. If it is considered that the pixel extraction accuracy is 1 individual pixel Words , The measurement accuracy is about 2mm. This precision is sufficient for SLAM Trajectory evaluation .

5. About the measuring range

If the camera is facing the measuring plane , The measurement area is a rectangle . If the camera is tilted , The measuring area is a general quadrilateral . The picture above is a kind of . Measure the size of the area and the angle of view of the lens 、 Camera mounting height 、 It all depends on the angle , More complicated . in general , In order to make the measurement accuracy in the measurement plane consistent , Try to keep the camera facing the measuring plane . The shorter the focal length , The larger the measuring field of view , However, the measurement accuracy decreases . In order to obtain sufficient measurement accuracy , You need a high-resolution camera . To lower the suspension height , You need a short focus lens . 

6. Code

 

All codes for calibration and measurement are as follows

https://github.com/ydsf16

The good news ！

Xiaobai learns visual knowledge about the planet

Open to the outside world

 download 1：OpenCV-Contrib Chinese version of extension module 

 stay 「 Xiaobai studies vision 」 Official account back office reply ： Extension module Chinese course , You can download the first copy of the whole network OpenCV Extension module tutorial Chinese version , Cover expansion module installation 、SFM Algorithm 、 Stereo vision 、 Target tracking 、 Biological vision 、 Super resolution processing and other more than 20 chapters .


 download 2：Python Visual combat project 52 speak 
 stay 「 Xiaobai studies vision 」 Official account back office reply ：Python Visual combat project , You can download, including image segmentation 、 Mask detection 、 Lane line detection 、 Vehicle count 、 Add Eyeliner 、 License plate recognition 、 Character recognition 、 Emotional tests 、 Text content extraction 、 Face recognition, etc 31 A visual combat project , Help fast school computer vision .


 download 3：OpenCV Actual project 20 speak 
 stay 「 Xiaobai studies vision 」 Official account back office reply ：OpenCV Actual project 20 speak , You can download the 20 Based on OpenCV Realization 20 A real project , Realization OpenCV Learn advanced .


 Communication group 

 Welcome to join the official account reader group to communicate with your colleagues , There are SLAM、 3 d visual 、 sensor 、 Autopilot 、 Computational photography 、 testing 、 Division 、 distinguish 、 Medical imaging 、GAN、 Wechat groups such as algorithm competition （ It will be subdivided gradually in the future ）, Please scan the following micro signal clustering , remarks ：” nickname + School / company + Research direction “, for example ：” Zhang San  +  Shanghai Jiaotong University  +  Vision SLAM“. Please note... According to the format , Otherwise, it will not pass . After successful addition, they will be invited to relevant wechat groups according to the research direction . Please do not send ads in the group , Or you'll be invited out , Thanks for your understanding ~