OpenCV hover-assist

Firmware/software/electronics/mechanics
twh
Beginner
Posts: 21
Joined: Tue Dec 06, 2016 7:34 am

Re: OpenCV hover-assist

Post by twh »

arnaud wrote:@pedro.lucas: Your messages kind of hijacked this thread so I have created a new thread to continue the discussion: viewtopic.php?f=6&t=2342.

@twh
I am currently looking at autonomous flight with a raspberry-pi camera V2 and opencv. I have that as a side-project and hopefully I can get something working soon :).

First of all you need to push meters in the kalman filter. Choose an origin and calculate the position of the Crazyflie in regards to this origin, in meters. Once that is done you will be able to push X and Y in the kalman filter, one way is to use the existing external position port. If you only get X and Y (and not Z) you can modify the part of the firmware that gets position update to only push X and Y in the EKF: https://github.com/bitcraze/crazyflie-f ... #L894-L901.

For Z with the barometer, it is not enabled yet in the EKF. So you will have to enable and test the pressure sensor.

Actually it would be easier if you could have X,Y and Z from your webcams, with 2 camera it should be possible, this way you do not have to rely on the pressure sensor for height.

Once you have the state estimator (the kalman filter) fixed, the controller will 'just work' no need to care about it. You can fly in "position hold" using a recent firmware and a recent client, we have added position hold to the client and it is activated with the same button as alt-hold.
Hi Arnaud,

I have finally rewritten my code for autonomous maneuvering inside the firmware. The designs are as follows,
1. 1 USB camera: 30fps I set the resolution to 1280 by 720. The distance between the Crazyflie and the camera is around 1 meter, so the x-axis is 1.2m for 1280pixel and y-axis is 0.6m for 720pixel. and the actual meters are fed into the firmware. (is this range too small?)
2. The origin (0,0) is always where the Crazyflie is located at the beginning, moving left increases x, moving right decreases x and moving up increases y
3. to track the Crazyflie using a coloured ball and send its x-axis and y-axis values based on its view via zmq to the client.
4. The client treats the received values as the y-axis and z-axis of the Crazyflie and send this value via port 6 to the firmware. Whenever the data is available, it will receive it and send it to firmware. (less than milliseconds)
5. Kalman filter is enabled to process the position values.
6. The pre-defined set point replaces the commandersetpoint so that it will take off for 1 second and do hovering for 2 seconds.
7. the antenna (in-between M1 and M4) of the Crazyflie is facing the camera and do the 2-axis control.

I have tested it these few days and did a couple of experiments. However, it couldn't work as it always drift to one side.
I am not sure whether I positioned the Crazyflie correctly? (which part of the Crazyflie should face towards the camera, e.g. xyz of the Crazyflie)
I have also tried to let the Crazyflie's antenna (in-between M1 and M4) facing away the camera and this seemed working but I still don't get it.
How do I know the Kalman filter with camera inputs works as expected? (is it checking the log of attitude.roll and pitch?)

Do you have any references that implement the system? It seems like I lack of relevant knowledge and have no very good understanding of the Crazyflie, especially the control and stabilisation system.

Sorry for the long question...
Hope you could help me clarify my doubts.

Thank you very much :D :D
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: OpenCV hover-assist

Post by arnaud »

One useful debug tool is to look at where the Crazyflie think it is by logging the kalman filter state: https://github.com/bitcraze/crazyflie-f ... 1289-L1291. If you have a recent client, this is displayed graphically in the LPS tab. It might be useful for you.

But the first things is to find out if the axis actually works.

The crazyflie boots with YAW=0 and facing X. The front is the antenna. So if you start the Crazyflie with the antenna facing away from the camera you will get:
- X going toward the depth.
- Y is the x axis in the camera.
- Z is the y axis in the camera.

So if you have a colored ball you could estimate x,y,z with X from the size of the ball and Y,Z from the position of the ball. OpenCV actually has function for calibrating the camera and calculating the 3D position.

Once you have your position estimation you can send it to the Crazyflie and observe how the kalman estimate is behaving, both when not flying and when flying.
twh
Beginner
Posts: 21
Joined: Tue Dec 06, 2016 7:34 am

Re: OpenCV hover-assist

Post by twh »

arnaud wrote:One useful debug tool is to look at where the Crazyflie think it is by logging the kalman filter state: https://github.com/bitcraze/crazyflie-f ... 1289-L1291. If you have a recent client, this is displayed graphically in the LPS tab. It might be useful for you.

But the first things is to find out if the axis actually works.

The crazyflie boots with YAW=0 and facing X. The front is the antenna. So if you start the Crazyflie with the antenna facing away from the camera you will get:
- X going toward the depth.
- Y is the x axis in the camera.
- Z is the y axis in the camera.

So if you have a colored ball you could estimate x,y,z with X from the size of the ball and Y,Z from the position of the ball. OpenCV actually has function for calibrating the camera and calculating the 3D position.

Once you have your position estimation you can send it to the Crazyflie and observe how the kalman estimate is behaving, both when not flying and when flying.

Thank you for your information.
I am still working on debugging it as it seems like my Crazyflie doesn't perform what is expected when running the position hold mode. Any parameters that I also need to take care of?

By the way, I would appreciate if anyone could share their design of camera-assisted autonomous system for Crazyflie. :D :D
I guessed It might be caused by my own design...

Thanks a lot. I will keep you guys updated about my progress in this post
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: OpenCV hover-assist

Post by arnaud »

Hi twh,

I finally got around to get my CF2 to fly with a webcam and the Kalman filter.

I am using the crayflie-ar-detector (I had to port it for openCV 3.2.0, the git repos is updated): https://github.com/bitcraze/crazyflie-ar-detector
I implemented a simple "extpos" tab that connects the detector and forward the position to the Crazyflie: https://github.com/ataffanel/crazyflie- ... dev-extpos
The crazyflie firmware is a stock build from master with "ESTIMATOR=kalman" in config.mk

It seems to work fairly well with the position-hold control introduced in recent version of the client and firmware.
I took a short video to show the performance: https://drive.google.com/open?id=0BxnrW ... 00xNmpzakU. The video is fully in position-hold mode, I made it move up and then a bit in X and Y during the flight. We still have some problem with the velocity output of the EKF and it makes the CF2 drop a bit over time, but globally it works well. The scatching sound is the propeller touching the paper tag during flight, this can be avoided by folding a bit the tag.

One bug that I have experience is that the EKF does not like to start with a negative Z, in my system I have to offset the Z measurement to keep it positive at all time.

To debug the system I have used the log variables ext_pos.X/Y/Z to check that the external position reaches the crazyflie and kalman.state_X/Y/Z to check that the kalman filter was converging towards the external position. I make sure to start the Crazyflie facing Z in my system (ie. a positive pitch should go towards X positive).

I am thinking of cleaning up the extpos tab and filling it up with debug tools that allows to observe the state of the crazyflie to undertand if things are working well or not.
twh
Beginner
Posts: 21
Joined: Tue Dec 06, 2016 7:34 am

Re: OpenCV hover-assist

Post by twh »

Oh great!
Sorry for late reply, I was kinda occupied these few weeks.
Maybe I need to check my code again as it is still not working and your work would definitely help me in this.

Will keep this post updated.

Thanks a lot~ :D
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: OpenCV hover-assist

Post by arnaud »

Hi,

Just an update, I have worked a bit further and have had very good result when using the LED ring as marker and putting a webcam under the Crazyflie. I made a detector for it: https://github.com/ataffanel/crazyflie-ledring-detector. I haven't had time to document it yet but I still plan to make a short video showing the performances :).

If you want to use the ledring detector, protip: as soon as the Crazylie enter the frame the kalman filter will freak out and output NaN, you need to reset it using the reset parameter. This is true for any detector apparently (I think it happens when the detector suddenly send a position that is far from x,y,z = 0,0,0), so this is good to keep in mind.

I am preparing an ext-pos tab that hopefully contains useful info and control for the task, do you see anything I should add?
extpos_prototype.png
Astrobiologist
Beginner
Posts: 25
Joined: Fri Jan 19, 2018 10:35 pm

Re: OpenCV hover-assist

Post by Astrobiologist »

Please could I ask a question about OpenCV, not hovering per se (the Crazyflie hovers very well now with the Flow Deck :D )

Since you guys have got OpenCV to work, I assume you must have installed it OK and hopefully will not mind giving me some hints.

I have tried sudo apt-get install python-opencv
It installs fine, but invariably as a Python 2 library not Python 3!

I have also cloned it from git, and then, from inside the folder, run
pip3 install -e .

- but again only Python 2!

I have found:
https://stackoverflow.com/questions/362 ... or-python3

But frankly this goes over my head and I don't really know what to type where.

How did you guys get it to install?

In any case, it does not seem to have been maintained since 2014, as far as I can see.

I was hoping to do some basic shape recognition in a script which also moves my Crazyflie about using mc.commander.

Any help gratefully received!
Thanks

Astrobiologist
(Oliver)
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: OpenCV hover-assist

Post by arnaud »

The only times I have used OpenCV with Python3 is when I have compiled it myself (often when trying to work on Raspberry pi).

The detector I shared in my previous message is running on Python 2.7. This is not a problem since I am using ZMQ to transmit the Crazyflie position to the client running in Python 3.

So my advice for now would be: if the version of openCV shipped with your Linux distro is good enough for you, it is easier to stick with Python2. Otherwise you will have to compile opencv anyway.
Astrobiologist
Beginner
Posts: 25
Joined: Fri Jan 19, 2018 10:35 pm

Re: OpenCV hover-assist

Post by Astrobiologist »

Upon some more searching around, this looks much more promising:

https://pypi.python.org/pypi/opencv-python

So just:
pip3 install opencv-python
(pip3 not pip because that is what you are likely to have installed when working through all the Crazyflie installation instructions)

...and it installs OK.
typing import cv2 works just fine from the prompt in Idle3.
An example script using cv2 is included below. This ran just fine from Idle3, and turned my webcam on etc.

There is still something up though since I was running Ubuntu in a VM and it crashed after a few tries!
Will report back.

I think the moral of the story for now is to avoid sudo apt-get install python-opencv for now.


import time
import cv2
camera_port = 0
camera = cv2.VideoCapture(camera_port)
time.sleep(0.1) # If you don't wait, the image will be dark
return_value, image = camera.read()
cv2.imwrite("opencv.png", image)
del(camera) # so that others can use the camera as soon as possible
Astrobiologist
Beginner
Posts: 25
Joined: Fri Jan 19, 2018 10:35 pm

Re: OpenCV hover-assist

Post by Astrobiologist »

Now trying to detect blobs that I could stick on my CrazyFlie (opencv for Python now seems stable!)
For instance:

# Standard imports
import cv2
import numpy as np;

# Read image from a jpg I have called blob.jpg
im = cv2.imread("blob.jpg", cv2.IMREAD_GRAYSCALE)

# Set up the detector with default parameters.
detector = cv2.SimpleBlobDetector()

# Detect blobs.
keypoints = detector.detect(im)

# Draw detected blobs as red circles.
# cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures the size of the circle corresponds to the size of blob
im_with_keypoints = cv2.drawKeypoints(im, keypoints, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)

# Show keypoints
cv2.imshow("Keypoints", im_with_keypoints)
cv2.waitKey(0)

But this keeps failing at the detect blobs step with the error "TypeError: Incorrect type of self (must be 'Feature2D' or its derivative)"
Presumably SimpleBlobDetector has been changed since the various tutorials were put up *.

Does anybody have any pointers?

Thanks

* e.g https://www.learnopencv.com/blob-detect ... -python-c/
Post Reply