Aruco using OpenCV on Raspberry Pi - Part 3 Detect marker and pose

okay in the last two videos we looked at how to set up your raspberry pi and then how to get your calibration matrix for the camera there's a fair bit of work but now that we've done it we never need to do it again we can just use that setup for all our future projects in this video we're going to look at how to detect the aruco marcus and how to work out your real world position based upon that detection or what's quite often referred to as the pose of the of the robot or the camera again there's a fair bit of technical stuff and i'm going to try to explain as best as possible what everything does if you don't understand everything it doesn't matter you can just cut and paste my code and believe it or not although what we're doing is really really complex there's very few lines of code in order to achieve it okay okay let's have a look at the robot itself all right up the top here i've mounted the raspberry pi with my screen on top and my camera out in front i've tried to mount my camera pretty much vertical over the top of the two claws that need to pick up the tennis balls and the base is an omni wheel base to give me three directions of freedom it's driven by stepper motors to give me very high accuracy so what i can do is i can use the high accurate stepper motors to navigate around when there's not an aruka marker in view and then as an aruka market comes into view i will use the aruka marker for the purpose of knowing where i am all right let's do a nice shot of that screen there we go running there at about 30 frames per second before we can actually detect the markers and estimate our pose from the marker we actually need some markers now where i got mine from was this website here right now when we go to the website there's a number of different dictionaries you can use i used the 4x4 dictionary but there are a number of different dictionaries for our fun our opencv function to detect the marker it needs to know which dictionary it's looking for so i did the 4x4 up to 250 up to 250 different possibilities now in order to estimate its pose it needs to know what size the marker is so i did mine a fairly standard size 100 millimeters by 100 millimeters when you print them out it means it will be 100 millimeters from there to 100 millimeters from there the bit you put on the floor will be bigger because it needs a bit of white around the outside to be able to detect the edge of black all right now each marker has an id and id this is id 0. we can go up this is id1 this is id2 this is id3 this is id4 so for this video i just printed out id 0 and id 1 the first two and then when we want to print them out you can just simply go open here hit print and he prints them out since last video i've done a little bit of cleaning up of this baruco directory what i've done is i've just simply deleted all the jpegs we used to calculate the calibration matrices and what i've done is i've made a backup copy in case i do something silly like accidentally delete that file let's now create some code to detect and estimate the pose let's call it detect dot pi and as always i have a bit of pre-made code pretty simple i'm going to import the needed modules i've made a variable called marker size that holds the size of the markers that i've made three lines of code to simply load up the two calibration matrices we need i've made another variable called a root aruco dict to hold the dictionary that i'm looking for and of course for me i'm using the 4x4 250 possibilities a line to open up the camera a few lines to set the camera up in the resolution the frame rate you want i'm just simply going to grab one single image i'm going to convert that image to grayscale and then i'm going to call this detect markers this detect markers i need to pass my gray scale image the dictionary i want it to try to detect and of course the two calibration matrices it will return a list of all the corners of all the markers it found it will return a list of all the ids of all the markers it found and also a list of rejected markers where it thinks it found a marker but it wasn't quite sure and we're not going to worry about that we're going to say if ivs is not none i.e if it did find some ides what we're going to do is we're going to call this draw detected markers and it will draw all the markers that are in this list of corners we pass the image we want to draw on the list of all the corners and it will draw a thin green box around all the markers in this list of markers next we're going to call this estimate pose single marker and what that will do it will estimate the three-dimensional pose of all the markers in it we're going to pass the list of corners i'm going to pass the marker size and then of course the two calibration matrices it returns a list of rotation vectors a list of translation vectors and a list of object points these two lists here is what we're interested in the translation vectors it's a list of vectors one vector for each marker it's found now the translation vectors tell you how far forward out of the camera to come how much for the left to go and how much to go up in order to get from the center of the camera to the center of the marker and once you get there this r back tells you how much to rotate in three dimensions to line yourself up perfectly square with the marker all right so later on we're going to use these two here to work out our real world coordinates but at the moment this is to go from the camera to the marker and square yourself up with marker just for the moment we want to draw the axis on these markers and also write the ids next to it so what i'm going to do is i'm going to loop through that list and i'm going to draw the access of each one of them again i need to pass the image on a drawer on the camera color the two camera calibration matrices and then each of the rotation vector and translation vector and also how long i want the axes to be on getting them to draw the axis axes 100 pixels long then the next one here is just to put the id the id on the marker next to the marker i might explain this in a second because it's a little bit of a pain in the ass to work out how i've managed to wrap this up in so many different things all right then i just simply show that frame wait for a key to be pressed and then close it all down right so let's hit save on that go across to my bnc viewer i've already opened up a terminal changed into that aruco directory so if i just simply run that program now i get this here so you can see it's drawn a green frame around all of them and i've got these two aruka markers the right way up orientate them right so this is the first corner it's marked here all right and it's drawn the axis in the three-dimensional axis and i've also got it to write the id number just in a little bit from the first corner all right and this here is aruca marker id 0 and this is aruka marker id1 to be able to get the pose of the robot in real world coordinates we need to understand the difference between the cameras coordinates and real world coordinates or rather how the camera sees the world and how the real world is so i've just googled camera coordinates to real world coordinates here's a nice little illustration down here that's come up so you can see from the camera's point of view where it's facing z is coming straight out of the camera but in the real world coordinates z goes vertically up it has its x in this direction but in real world coordinates x is in this direction its y is in this direction and in real world coordinates it's in this direction now the one good thing between these two coordinate system is one millimeter for the camera is millimeter in the real world that its coordinates are also all 90 degrees apart and these are all 90 degrees apart the only difference between the camera coordinates and real world coordinates is orientation the scale is the same and they're not skewed they're just in different orientations i've cut down detect pi a fair bit here i've removed out where we cycle through detect all the access draw all the access on draw all the id markers on and what i've done is i've just gonna still call this estimate pose single markers what i'm going to do is i've changed it from rvac to rvac list all and from t vector t list all because that's what really these are they're not a single translation vector but rather a list of all the translation vectors it's not a single rotation vector but a list of all of them so the first thing so i can understand them a little bit better i'm just going to print them out simply show the frame wait for a key to press and then close everything down when i show the frame the only thing that will be on the frame is the drawing of the detector markers so let's save this let's flip over to here and run that all right so we can just simply see it's detected the markers drawn the green box around them if i press a key we can see the output here so it outputs two vectors right because there's two markers all right so we have this top vector here this bottom vector here now a couple of things you might notice is that this vector is wrapped in another vector and the same with this bottom one here this vector here is wrapped in another vector and then the two vectors are wrapped in another one all right so let's start pulling that apart so i come back to here instead of list print list all if i just simply print print the very first one all right let's see what we get with that if i come up to here switch across press a key so now i just get one vector but you can see that vector is wrapped in another box so what we're going to do now is i don't know why they've done this but we need to then take the next one just the first element of that so let's now save that run that get the same with those we push it off what we get now is the translation vector of the very first one string where the x is equal to t vector the y is equal to t back one and the z is equal to t back to and i'm simply going to put that text on the frame this is the text i'm putting on 20 in 460 downs near the bottom of the image in that font twice as big in red twice as thick in that style all right so let's have a bit of a look at what that looks like let's put him into the center here right so let's see if we can actually line him up in the center of the image so what this is saying is from the camera we have to come in the z directions directly out of the center of the camera 513 millimeters then we actually have to come up 20 millimeters because from the camera point of view the low values of wire at the top of the screen the high values are white at the bottom of the screen the low values of x is that side of the screen and the y at the high values of x is that so we're coming out 515 up 20 mil and then across 20 mil so if we shift him across here to the side and this time it's saying that we need to come forward 500 millimeters we need to come down two millimeters and we need to come back negative 139 millimeters right right now we have to talk about rotation vectors they took me a bit to get my head around if you don't understand them it's okay this part you can just cut and paste my code if i'm looking at this one here which rotates just around that axis then i'll move across the next one which rotates around this axis both those rotations one after the other is equivalent to one single rotation around an off axis axis 45 degrees between the two and that's called a rotation vector the rotation vector will be in the direction of that up that axis that the rotation comes around and then the length of the vector will be the magnitude or the amount of the rotation when you get that vector and you flip it in the other direction the the axis of rotation is exactly the same just the direction is in the opposite direction right going back to this camera coordinates to real world coordinates at the moment we have a translation vector that will take us from the camera and translate us to the center of the marker then we have a rotation vector that will rotate us to now we're facing in the same direction as the marker coordinates if we want to go from the marker back to the camera what we need to do is we need to rotate around the same axis but in the opposite direction to go back to where we were or just flip the rotation vector the other way and then we need to translate backwards or just flip the translation vector um backwards right so we can un-rotate and un-translate but still because we've un-rotated when i'm doing this translation the translation isn't in the real world coordinates of rotated and they're back in the camera coordinates so what we need to do is we need to have a change of base from the camera coordinate system to the real-world coordinate system i have this awesome video that explains it how to translate back and forth between coordinate systems using different base vectors and basically all you do if you have a vector in one coordinate system and you want to get the vectors in a different coordinate system you just need to multiply that vector by the transformation matrix now we worked out before to go from the matrix to the camera it's just the rotation vector flipped but that's a rotation vector not a rotation matrix now a rotation matrix is the same thing but in different form so it's like having one quarter and 0.25 they're both the same thing but written in different form so the first thing we need to do is court is trans and change it from a rotation vector into a rotation matrix and the formula for doing that is the rodriguez rotation formula now the great thing is all this stuff is built into python we're going to do all of this in two lines of code all right but these two lines of code have this huge big complex explanation behind them okay looking at my robot it can move forwards and backwards in the y can move side to side in the x but it is unable to fly and move up and down in the y so i don't need to know the y now with rotation it can rotate in your but the robot is unable to rotate in pitch or rotate in roll so the three pieces of information i want to know is what it's x or sorry what it's y is what it's x is and what it's your angle is all the rest i don't need to know right so what we're after is to know the x and y position in the real world and the angle or the direction the robot's facing in the real world we have currently already worked out the x position the y position in the real world now we need the angle we can get the angle from one of two places either get the angle from the rotation vector or from the rotation matrix it is easier to get it from the rotation matrix now i have just cut and paste someone else's code here that does that and i simply got that from this website here this guy here does a great job of explaining how to do it and why it works and provides the code i just cut and paste the code here right you'll also need to add import math because he uses the math so scrolling down where we were before i've added this one more line i've said the pitch the role on the or will be equal to that function i just cut and paste up the top from that website and i pass it my rotation matrix and what i've done is i've updated my string here i'm going to put on the screen i said the x is still equal to the real world t back zero the y is equal to the real world one but now i've written here the direction is equal to the your i've got back there now this here sorry this here returns in radians just to make it easier for people to understand i'm using math.degrees to convert it from radians to degrees when i write it on the screen let's have a little bit of a look-see at this let's run that again there he is there so now i'm getting the x position the y position and the direction all right so if i manage to keep the camera uh this we're actually bringing forward a little bit if i can try to keep the camera in the same spot and rotate the um the robot something like that the x and the y should have stayed relatively the same is just now facing 36 degrees so if i push the robot directly sideways without rotating it what should happen is the y shouldn't really change in the direction should have really changed just the x will change about the best i could do if i bring it in closer and take it further away the x shouldn't change but the y will change in the direction will stay much the same doing that the best i can of course everything's in millimeters for me by hand to do that really accurately down to the millimeter isn't the s so now we have the real world pose of the robot the x the y and the direction extracted from the euruko marker

Channel: Out of the BOTS

Views: 10,427

Keywords:

Id: cIVZRuVdv1o

Length: 21min 23sec (1283 seconds)

Published: Sat Jul 17 2021