Using v2 lighthouses facing upwards

Topics related to the Lighthouse positioning system, configuration and use
cafeciaojoe
Member
Posts: 83
Joined: Mon Jun 18, 2018 2:37 am

Re: Using v2 lighthouses facing upwards

Post by cafeciaojoe »

Hi Kristoffer,

I checked out the latest version of the lighthouse positioning tab today in response to Tobias's battery test function blog post.

It looks like the calibration and geometry data are no longer hard coded into the firmware?
How do you define the origin now?

I assume there is some documentation coming house soon for the new tab, so I am happy to just wait for that if it is coming soon. I just want to guage wether or not I move to this version of the cf-lib and draw inspiration from the lighhouse positioning tab for our solution? Rather than our current solution which is building a script that will generate an origin and rotation matrix that will later be hard coded.

Cheers
Joe
PhD Student
RMIT University
School of Design
http://www.cafeciaojoe.com
Image
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Using v2 lighthouses facing upwards

Post by kristoffer »

Hi Joe,
yes, we are implementing a proper user interface for managing lighthouse systems. Geometry and calibration data will be stored in the persistent memory in the CF instead of being hard coded in the firmware. This means the lighthouse system can be used with the stock firmware and there will no longer be necessary to build a special lighthouse firmware. The UI will be limited to 2 base stations for now but we will add support for 2+ base stations later. We want to get a first official version out and I think 2 base stations is the most common use case so we settled for that. Geometry estimation for 2+ base stations is more complex (as you know :-) ) and we will have to contemplate a bit on how to do that.

Even though the UI is designed for 2 base stations, most of the functionality (in CF and client) should actually work for 2+ base stations as well. In your case I think using the persistent storage for geo and calib data would be a convenient solution. You could probably use the stock firmware which would simplify things.

The geometry data is read from the persistent storage at start up (if available). The main use case for calibration data is to read it from the light sweeps and store it to persistent memory to make it available immediately after boot. If you want to read calib data from the USB on the base stations, that is also fine. Note that we added a UID to the calibration data.
If you have more than 2 base stations, I have a slight worry that there will be interference between base stations when receiving calibration data through the light sweeps. If this is a problem turn on one base station at a time and let the CF get the calib data (you will get a notification in the console log and the lighthouse.bsCalCon log variable) which automatically will be stored in persistent memory.

There is now a system config file format that contains geometry and calibration data, that enables a user to save and load full system configurations. The easiest way for you to understand the file format is probably to save a file from the lighthouse tab and look at the file. It is pretty straight forward.
I'm not sure how you designed you tool, but this file format may be a way for you to transfer information?

There is a script in the crazyflie-firmware repo (tools/lighthouse/upload_config.py) that reads a system config file and writes it to the CF that might be useful to you.

Documentation is on its way, but we want to finish the implementation first.

Let me know it you want to set up a meeting for a more detailed explanation
cafeciaojoe
Member
Posts: 83
Joined: Mon Jun 18, 2018 2:37 am

Re: Using v2 lighthouses facing upwards

Post by cafeciaojoe »

I can't even get the drone to fly with the latest pull, with unmodified firmware, client and cflib... So i will try to get it fly on a multi bs setup with hard coded geo/calib values first. Then we can talk about moving it over once the tab/lib and firmware is more stable and there are docs...

Sould like a good idea to have my script spit out values in the form of the .yaml system config file. I will aim for that.

Cheers
Joe
PhD Student
RMIT University
School of Design
http://www.cafeciaojoe.com
Image
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Using v2 lighthouses facing upwards

Post by kristoffer »

That sounds bad! I will hopefully spend some time in the flight lab tomorrow and will do some testing
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Using v2 lighthouses facing upwards

Post by kristoffer »

I tried it in our flight lab on Friday and it seemed to work fine both with V1 and V2. What kind of problems did you see?
cafeciaojoe
Member
Posts: 83
Joined: Mon Jun 18, 2018 2:37 am

Re: Using v2 lighthouses facing upwards

Post by cafeciaojoe »

I tried again in our lab, clean install new drone. all works ok! So not sure what that was.

Just waiting now for my engineer friend to finalise the modifications to the

Code: Select all

class LighthouseBsGeoEstimator
before we move on to integrating it into the newest version of the Lighthouse Tab.

J
PhD Student
RMIT University
School of Design
http://www.cafeciaojoe.com
Image
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Using v2 lighthouses facing upwards

Post by kristoffer »

Sweet!

We are very close to finishing the SW for the first release of lighthouse, and the code should be more stable.
cafeciaojoe
Member
Posts: 83
Joined: Mon Jun 18, 2018 2:37 am

Re: Using v2 lighthouses facing upwards

Post by cafeciaojoe »

Hi Kristoffer!

My engineering friend Tim has worked on this problem for the last couple of weeks in his spare time. We are at the point where we would like you to review the work and see if there is anything obvious that might have been missed. His message Below:
We’re trying to design and implement a system where we have BS’s on the ceiling and floor, to support tracking when the CF is upside down. We’ve gotten advice in previous posts about doing this, and our current high level approach is the following:

Update get_bs_geometry such that it’s able to accurately estimate BS locations with the CF on it’s side (as a sideways orientation puts the sensors in view of both BS’s on the floor and ceiling)
Be able to stitch together multiple runs of get_bs_geometry to put all the BS’s into a single world reference frame, as not all BS’ will be able to be in view of the CF sensors in just one run.

This post is basically to check in and make sure our approach for (1) makes sense, and we haven’t made any errors in interpreting the current code. Our approach is laid out in a series of Jupyter notebooks:

(1)Finding the initial yaw guess for the CF. https://github.com/cafeciaojoe/crazyfli ... uess.ipynb
(2) Converting the initial yaw guess to an initial pose guess for the OpenCV solver. https://github.com/cafeciaojoe/crazyfli ... n-cv.ipynb
(3) Solving for the BS’s pose with OpenCV https://github.com/cafeciaojoe/crazyfli ... -pnp.ipynb

(Note you may have to hit “reload” a few times on the notebooks in GitHub to get them to work, and that the 3D plots won’t render in GitHub)

We’ve tried to lay out our approach and assumption as clearly as possible - would it be possible to take a look at these, especially (1) and (2), to sanity check what we’re doing? We’ve done a fair bit of interpreting of the current code and what it’s doing in order to make changes, and want to make sure our interpretation is correct.
Also, if you have the time we should probably just meet over a video call :D

Thanking you in advance!

Joe and Tim
PhD Student
RMIT University
School of Design
http://www.cafeciaojoe.com
Image
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Using v2 lighthouses facing upwards

Post by kristoffer »

Hi Joe and Tim!

I have done some more testing and thinking and as far as I can see the estimated position of the base station is mirrored when the base station is upside down. In this case, the correct position seems to be (-X, -Y, Z) of the estimated position. I have not looked at the rotation, but I would expect it to be some sort of mirror solution as well.

The reason we get this is that the initial guess is wrong since the sweep is going in the "wrong direction" when the base station is up side down, and the estimation function in open CV ends up in the wrong local minima.

So when is the base station upside down? I think it is upside down when the Z axis of the CF and Z axis of the BS are pointing "in opposite directions", (Z_CF dot Z_BS < 0).

I looked at the pictures in (1) and I think that when the CF is on the side (Z-axis along the floor) the solution for the BS on the ground will be OK with the current code, while the BS on the tripod will get a mirror solution. Correct?

Maybe you can exploit this if you know which base station that is on the tripod?

Anyway, your suggested solution looks reasonable, even though I have not tested it. I would have expected that you only had to do something for the initial guess, but maybe the rotation must be handled in some way as well?

Let's have a meeting and discuss it, just drop me an email!

br
Kristoffer
cafeciaojoe
Member
Posts: 83
Joined: Mon Jun 18, 2018 2:37 am

Re: Using v2 lighthouses facing upwards

Post by cafeciaojoe »

Hi all,

After all these heavy modifications to get_bs_geometry.py using Kristoffer Script from early in this thread we pulled something together that got the BS in kind of the right place, but were having serious problem with the averaging of the quaternions.


Then we realised (after installing all the lighthouses) that if you just raise the lighthouses a little more but still have them angled up. you can just use the manage geometry window to calibrate in one styingle step... :oops:

But I guess all this had to happen for anyone to realise this. Tims and Kristoffers work on this is still really valuable to anyone who wants to stitch together multiple coordinate system (especially across multiple rooms), there mus't be that far to go.

they sit about 400mm off the ground and are angled up about 10 degrees.

Wow what a ridiculous Journey... Thank you all for your help.

Now on to the next promlem... when we turn on over 4 Lighthouses the CF does not recieve any of them :twisted:
Attachments
Screen Shot 2021-05-10 at 5.28.11 pm.png
PhD Student
RMIT University
School of Design
http://www.cafeciaojoe.com
Image
Post Reply