Using v2 lighthouses facing upwards

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

Using v2 lighthouses facing upwards

Post by cafeciaojoe »

Hi,

I am looking for some guidance on how to to run >2 V2 lighthouses, some will be mounted on the ceiling looking down. Others will be mounted on the floor looking upwards. (there will be two motorless crazyflies that need to be tracked even when upside down see here: viewtopic.php?f=20&t=4600)

The first part of this question is how to move past 2 base stations. The documentation talks about modifying "PULSE_PROCESSOR_N_BASE_STATIONS in pulse_processor.h ". So I assume after doing this, the 8 step process in the documentation is the same. https://www.bitcraze.io/documentation/r ... etting_up/

with regards to the upwards facing base stations, Is is as simple as running get_bs_geometry.py to get the positional data with the drone upside down? Then inverting these values in some way, before adding them to the positional data retrieved from the drone facing the right way up?

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

Re: Using v2 lighthouses facing upwards

Post by kristoffer »

This is not really supported (which makes it more fun of course)

If you increase PULSE_PROCESSOR_N_BASE_STATIONS to say 4, the Crazyflie will support 4 base stations, using channels 0-3 (1-4 in the lighthouse GUI).
Everything else is identical to 2 base stations.

For the get_bs_geometry.py script, this is where it is getting more tricky. The script supports 2+ base stations so that is fine. The functionality of the script is to estimate base station positions and orientation in relation to the Crazyflie, and the output is based on a coordinate system with the CF in the origin and with the same orientation as the CF. This works well when all base stations are visible from the origin, but it does not when the system is larger or in your case when some base stations are facing upwards.
In a larger system where all base stations are not visible from the origin, it is possible to make multiple measurements with the Crazyflie at different positions and "stitch" the solutions together into one coordinate system by rotating and translating the solutions into a global coordinate system. We do not have a script for this at the moment, but I have done some experiments and it is possible.

You may also run into another problem in your case is that the base stations on the floor is facing upwards and the the CF will not be flat on the floor when running the get_bs_geometry.py script. The script is using open CV to estimate the base station geometry and we feed it with an initial guess of the direction to the base station. I think the initial guess is based on the assumption that the CF is on the floor, and it is possible that you will get some sort of mirror solution instead of the real one.
cafeciaojoe
Member
Posts: 75
Joined: Mon Jun 18, 2018 2:37 am

Re: Using v2 lighthouses facing upwards

Post by cafeciaojoe »

Hi Kristoffer!

Thanks for the reply. I have three Q.

1) what do you mean by
kristoffer wrote: Fri Jan 29, 2021 9:16 am using channels 0-3 (1-4 in the lighthouse GUI).
Are you reffering to the HTC software?

2)
kristoffer wrote: Fri Jan 29, 2021 9:16 am In a larger system where all base stations are not visible from the origin, it is possible to make multiple measurements with the Crazyflie at different positions and "stitch" the solutions together into one coordinate system by rotating and translating the solutions into a global coordinate system
I assume this means getting the geometry data and inverting the three 'origin' values as needed (by changing the values from + to -), and inverting the rotational 'mat' values. Is there anymore informatin on these mat values?

3)
kristoffer wrote: Fri Jan 29, 2021 9:16 am I think the initial guess is based on the assumption that the CF is on the floor, and it is possible that you will get some sort of mirror solution instead of the real one.
So, if I built a stand that held the deck in the same x,y,x position. 1.5m off the ground. But in two orientations, (facing up and facing down).
Then the origin of the space would be [0,0,0] but placing the drone on the floor would be [0,0,-1.5] Would this solve this problem?
PhD Student
RMIT University
School of Design
http://www.cafeciaojoe.com
Image
kristoffer
Bitcraze
Posts: 464
Joined: Tue Jun 30, 2015 7:47 am

Re: Using v2 lighthouses facing upwards

Post by kristoffer »

1) Valve is using the values 1 to 16 when configuring the channel in a Lighthouse V2 base station. It is also possible to set it to 0, but that means "detect which channels that are used and pick one that is unused", and this feature should not be used with the Lighthouse deck.
On the other hand, in the Crazyflie firmware the channels are zero-indexed and values are in the 0-15 range.
I was mainly referring to the terminal UI that is available when connecting to a base station via USB, but I assume the channel numbering is the same in all Valve (HTC) GUIs.

2) In you case, if you keep the Crazyflie in the same position and flip it 180 degrees that should be true (I think).

3) That is correct. You can translate the base station positions 1.5 m if you want the origin to be on the floor though.
cafeciaojoe
Member
Posts: 75
Joined: Mon Jun 18, 2018 2:37 am

Re: Using v2 lighthouses facing upwards

Post by cafeciaojoe »

Hello.

I have been refreshing my knowledge on rotation matricies and wanted to propose a better way to mesh these coordinates together.

If we took three readings from get_bs_geometry.py in three seperate positions. we could chain the reference frames together right? (it has been a long time since I did engineering maths)
IMG_6414.jpeg
This would save me the tricky task of making sure the CF is the same position for each measuremnt (as initially suggested)

! am going to look into calculating this tomorrow, but I got some weird results when I took the measurements today with the Cf on it's side. I was getting seemingly inverted Y axis values for geo.origin... the y value should be negative shouldnt it? assumign the right hand rule?
IMG_6415.JPG
PhD Student
RMIT University
School of Design
http://www.cafeciaojoe.com
Image
kristoffer
Bitcraze
Posts: 464
Joined: Tue Jun 30, 2015 7:47 am

Re: Using v2 lighthouses facing upwards

Post by kristoffer »

Hi!
If we took three readings from get_bs_geometry.py in three seperate positions. we could chain the reference frames together right? (it has been a long time since I did engineering maths)
Yes, as long as you have readings for (at least) two base stations in each position you can map the pose of a base station from one position to another.
Each CF position is a reference frame and you need to find the transformation of the base station pose from one reference frame to the next one.

You can try this script out, or use is as inspiration.

Code: Select all

import math
import numpy as np
from scipy.spatial.transform import Rotation
from collections import namedtuple

System = namedtuple("System", ['P', 'R', 'bs'])

def gen_rot_x(angle):
    co = math.cos(angle)
    si = math.sin(angle)

    return np.array([
        [1, 0, 0],
        [0, co, -si],
        [0, si, co]])


def gen_rot_z(angle):
    co = math.cos(angle)
    si = math.sin(angle)

    return np.array([
        [co, -si, 0],
        [si, co, 0],
        [0, 0, 1]])

# Transform a point in ref frame x to ref frame r using transformation x->r
def transform_point_x_to_r(v_x, Px_r, Rx_r):
    return np.dot(Rx_r, v_x) + Px_r

# Transform a rotation in ref frame x to ref frame r using transformation x->r
def transform_rot_x_to_r(Rx_y, Ry_r):
    return np.dot(Ry_r, Rx_y)

# Transform a base station (y) in ref frame x to ref frame r using transformation x->r
def transform_x_to_r(Sy_x, Sx_r):
    Py_r = transform_point_x_to_r(Sy_x.P, Sx_r.P, Sx_r.R)
    Ry_r = transform_rot_x_to_r(Sy_x.R, Sx_r.R)
    return System(Py_r, Ry_r, Sy_x.bs)

# Transform a point in ref frame r to ref frame x using transformation x->r
def transform_point_to_x_from_r(v_r, Px_r, Rx_r):
    Rr_x = np.matrix.transpose(Rx_r)
    return np.dot(Rr_x, v_r - Px_r)

# Find the transformation x->r when we know the bs pos/rot for both x and r
def transform_from_ref_x_to_r_same_bs(Sb_x, Sb_r):
    Rb_x_inv = np.matrix.transpose(Sb_x.R)
    Rx_r = np.dot(Sb_r.R, Rb_x_inv)
    Px_r = Sb_r.P - np.dot(Rx_r, Sb_x.P)

    return System(Px_r, Rx_r, Sb_x.bs)


# Averaging of quaternions
# From https://stackoverflow.com/a/61013769
def q_average(Q, W=None):
    if W is not None:
        Q *= W[:, None]
    eigvals, eigvecs = np.linalg.eig(Q.T@Q)
    return eigvecs[:, eigvals.argmax()]

def system_average(S):
    bs = S[0].bs
    for s in S:
        if s.bs != bs:
            raise Exception("Different base stations")

    Q = map(lambda s : Rotation.from_matrix(s.R).as_quat(), S)
    q = q_average(np.array(list(Q)))
    r = Rotation.from_quat(q).as_matrix()

    P = map(lambda s : s.P, S)
    p = np.average(np.array(list(P)), axis=0)

    return System(p, r, bs)



measurements = [
    [
        # The global ref frame - the origin
        System(np.array([-1.4852667 ,-0.10013239, 2.0018834 ]), np.array([[ 0.71420725,-0.44233498, 0.5424461 ], [0.3414208 ,0.89670753,0.28168679], [-0.61101542,-0.01598036, 0.79145738], ]), 2),
        System(np.array([ 2.62930353,-0.22387827, 2.2876007]), np.array([[-0.44243481,-0.66061902,-0.60649316], [ 0.51203157,-0.74130071, 0.43393194], [-0.7362575 ,-0.11855705, 0.66623504], ]), 1),
    ],
    [
        System(np.array([ 2.63076761,-0.22235061, 2.27780455]), np.array([[-0.44400779,-0.66127997,-0.60462044], [ 0.51142969,-0.74110235, 0.43497929], [-0.73572872,-0.11608665, 0.66725343], ]), 1),
        System(np.array([-1.4770189 ,-0.09919998, 2.00858532]), np.array([[ 0.71191171,-0.44151409, 0.54611997], [0.34088548,0.89714765,0.2809327 ], [-0.61398599,-0.01383492, 0.78919566], ]), 2),
    ],
    [
        System(np.array([1.8918052 ,0.37315688,2.2956454 ]), np.array([[-0.55202334,-0.43387006,-0.71205828], [ 0.36222251,-0.89395567, 0.26389036], [-0.75104266,-0.11224989, 0.6506419 ], ]), 1),
        System(np.array([-2.06908923,-0.60137163, 2.00787968]), np.array([[ 0.60031459,-0.6606275 , 0.45077011], [0.50542771,0.75018765,0.42633475], [-0.61981063,-0.02810327, 0.78424804], ]), 2),
    ],
    [
        System(np.array([0.81226372,0.72658043,2.30703026]), np.array([[-0.63100663, 0.18127552,-0.75430088], [-0.02502271,-0.97656606,-0.21375824], [-0.77537378,-0.11600822, 0.62075566], ]), 1),
        System(np.array([-1.72116567,-2.38984028, 1.9674524 ]), np.array([[ 0.18445731,-0.97738034, 0.10345616], [0.76810961,0.20902735,0.60523978], [-0.61317463,-0.03217523, 0.78929185], ]), 2),
    ],
    [
        System(np.array([0.88302677,0.21773784,1.29904   ]), np.array([[-0.82446229, 0.23252554,-0.51593973], [-0.23580717,-0.96992637,-0.06031429], [-0.51444816, 0.07193542, 0.85449891], ]), 0),
        System(np.array([-0.34440593,-1.80757135, 2.14094036]), np.array([[-0.11239338,-0.94578565,-0.30472484], [ 0.69138764,-0.29469574, 0.65964957], [-0.71368821,-0.13654274, 0.68702636], ]), 1),
    ],
    [
        System(np.array([-0.08719829, 1.03855911, 1.26059635]), np.array([[-0.21572911, 0.94116655,-0.26012782], [-0.83978171,-0.31475733,-0.44237372], [-0.49822448, 0.1230177 , 0.85827677], ]), 0),
        System(np.array([ 0.95282395,-1.10080986, 2.20056662]), np.array([[-0.61788579,-0.2604643 ,-0.74187296], [ 0.27185388,-0.9561156 , 0.10926308], [-0.73777545,-0.13416894, 0.66157848], ]), 1),
    ],
    [
        System(np.array([-2.08526947,-0.42735793, 1.1706371 ]), np.array([[ 0.566362  ,-0.74323252, 0.35614536], [0.69140488,0.66366893,0.28548703], [-0.44854585, 0.08455164, 0.88975145], ]), 0),
        System(np.array([1.82680506,2.06461834,2.15551761]), np.array([[-0.6515338 , 0.50143071,-0.5692723 ], [-0.37619232,-0.86519582,-0.3315351 ], [-0.6587739 ,-0.00185046, 0.75233871], ]), 2),
    ],
    [
        System(np.array([-1.12594278,-2.05115544, 1.18245119]), np.array([[-0.15492359,-0.98792286,-0.00266465], [ 0.87068111,-0.13781152, 0.47214658], [-0.46681162, 0.07082658, 0.88151603], ]), 0),
        System(np.array([-0.5475143 , 2.51547338, 2.07912607]), np.array([[-0.11243505, 0.98492681,-0.13144404], [-0.74911736,-0.17092836,-0.64000522], [-0.65282582, 0.02650799, 0.75704411], ]), 2),
    ],
]

##################

def print_system(system):
    print(f"Base station {system.bs} @ {system.P}")


def probe_position(Sbs_ref, Sbs0_other, Sbs1_other):
    # Find transform from other ref frame to global using bs0
    Sother_g = transform_from_ref_x_to_r_same_bs(Sbs0_other, Sbs_ref)

    # The meassurement of base station 1 in the other ref frame, converted to global
    Sbs1mOther_g = transform_x_to_r(Sbs1_other, Sother_g)

    return Sbs1mOther_g


print()
ref = measurements[0][0]

result = [ref]
found = [ref.bs]

not_done = True

while not_done:
    not_done = False
    samples = {}

    print("--- iteration")

    for measurement in measurements:
        # Find a reference system in this measurement
        from_bs = None
        from_sys = None
        from_sys_g = None
        for system in measurement:
            bs = system.bs
            for sys in result:
                if sys.bs == bs:
                    from_bs = bs
                    from_sys = system
                    from_sys_g = sys
                    break
            if from_bs is not None:
                break

        # Transform all base stations in this measurement to
        # the global system, unless we already have a result for a
        # particular base station
        if from_bs is not None:
            print(f"Using {from_bs} as reference")
            for sys in measurement:
                if sys is not from_sys:
                    if sys.bs not in found:
                        s = probe_position(from_sys_g, from_sys, sys)
                        from_to = (from_bs, sys.bs)
                        if not from_to in samples:
                            samples[from_to] = []
                        samples[from_to].append(s)
        else:
            not_done = True

    # Average over all samples sets
    for from_to, sample_set in samples.items():
        new_sys = system_average(sample_set)
        result.append(new_sys)
        found.append(new_sys.bs)


for sys in result:
    print_system(sys)
I got some weird results when I took the measurements today with the Cf on it's side. I was getting seemingly inverted Y axis values for geo.origin... the y value should be negative shouldnt it? assumign the right hand rule?
The layout of sensors on the deck (symmetrical and in one plane) will give us multiple possible (mirrored) solutions to the base station pose. To find the right one we feed the estimator with an initial guess, but that guess is based on the assumption that the crazyflie is flat on the ground and that the base stations are located above the ground (z > 0). It is possible that you find a mirror solution.

In your case (the picture) I think i looks like you should get something like (-1, 0, 2). You can find the definition of the coordinate system here https://www.bitcraze.io/documentation/r ... finitions/
cafeciaojoe
Member
Posts: 75
Joined: Mon Jun 18, 2018 2:37 am

Re: Using v2 lighthouses facing upwards

Post by cafeciaojoe »

Hello!

So I had a look at the script you posted (thank you)

I did two things here.

1) Took 4 measurements in various places on the floor with the CF facing upwards, and 4 measurements in various places on a platform 1.2 m high. The script does seem to smooth out a lof of the jumping around I would normally get if I took one 'get_bs_geometry.py' measurement fromt he floor. See below, the plot of the CF just sitting on the floor withoug having taken off. IE the CF flies more smoothly with the multiple measurments.
with a single run on get_bs_geometry.py (smooth flight)
with a single run on get_bs_geometry.py (smooth flight)
with multiple runs of get bs_geometry.py and the script in the previous thread. (rough flight)
with multiple runs of get bs_geometry.py and the script in the previous thread. (rough flight)
2) When I took 6 measurements with the Cf on it's side + 2 measurments with the cf facing up, I also got the same kind of stability improvement, but once i flipped the motorless cf 'hand pad' over (to rely on the single BS on the floor facing upwards), the x and y state estimates seem to be tracking the z values. Below, I am moving the drone up and down in the same global x,y but +-1.2 m in the z axis.
moving the drone up and down in the global z.
moving the drone up and down in the global z.
Can this script handle measurements taken from the CF on it's side? From what I can tell it should be able to. The rotation matricies were multiplied to transform them into the global system. that should take into account any position the CF finds itself in...?

the other thing about this script I dont understand is why {system.R} is not returned at the end... are the rotation matricies not supposed to be used?

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

Re: Using v2 lighthouses facing upwards

Post by kristoffer »

Hi Joe!

As mentioned earlier, the geometry estimation script is designed for a CF placed on the floor. Any other use cases may produce unexpected results and I do not know what to expect.
The script I provided for multiple geo measurements was a quick hack to try out the concept, there are no guarantees that it is working or that the API is useful. I'm happy it seems to produce something that is useful at least in some cases :-)

You are braking new ground here, and unfortunately I don't have that much to add. I think your first step should be to figure out if the geo estimation script produces any useful results with the CF on the side or up-side-down. If it doesn't you will have to start by fixing that.
When you know your geo data is correct you can proceed to figure out how to stitch the solutions into one global coordinate system.

Stitching multiple geo measurements is something we probably will do in the future to support 2+ base stations, but I don't know when.
cafeciaojoe
Member
Posts: 75
Joined: Mon Jun 18, 2018 2:37 am

Re: Using v2 lighthouses facing upwards

Post by cafeciaojoe »

Hi Kristoffer,

Super exciting to have finally reached some precipice!

I have an engineer friend to help me from this point forward. He wants to know how does the rotor reference frame come into play.
is it something we need consider in this problem?

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

Re: Using v2 lighthouses facing upwards

Post by kristoffer »

Hi Joe!

No, you should not have to care about the rotor reference frame, only the CF-, global- and base station reference frames.
The output from the estimation script is the base station pose (position + attitude) in the CF reference frame.

In the standard use case the pose of both base stations are estimated at the same time, and thus share the same (CF) reference frame. Further more the CF reference frame (at the point of estimation) is considered to be the same as the global reference frame and defines the global coordinate system.

In your case you will have multiple base station estimates done at different times with different CF reference frames (as the CF has moved around). You will also have to decide where you want to define your global reference frame.

- K
Post Reply