Get the proper SimpleBlobDetector blobs order to use with solvePnP

Python
, ,
1

Hi there!

The point of the project

I’m currently trying to create a suitable infrared LED position tracking system for my project.
As the base layer I’m using the SimpleBlobDetector to detect the 4 bright LEDs “marker” on an under-exposured video feed. Then I have a points_3D matrix which represents the physical dimensions of my marker and relative distance between the LEDs to use in solvePnP.

Code

The simplified code looks something like this, assume the camera capture, blob detector and some other params are predefined:

# The matrix which row represents each LED relative position as (X, Y, Z) offsets
points_3D = np.array([
    (0, 0, 0),      # Blob Zero
    (0, -4, 0),     # Blob 1 is 4 units UP from zero
    (-2.2, -4, 0),  # Blob 2 is 4 units UP and 2.2 units LEFT
    (-7.4, -4, 0),  # Blob 3 is 4 units UP and 7.4 units LEFT
], dtype="double")

# A helper matrix to draw the marker position and rotation (as XYZ axes), used for a drawing function I'm not displaying here
axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3)

# Detecting the LED light as blobs, this all works nice
keypoints = detector.detect(prepeared_frame)

# Converting to simple points array
points_2D = np.array([point.pt for point in keypoints], dtype="double")

# Only solving PNP when all 4 points are available
if len(points_2D) == 4:
        success, rotation_vector, translation_vector, _ = cv2.solvePnPRansac(
            points_3D,
            points_2D,
            calib_matrix,
            dist_coeffs,
            cv2.SOLVEPNP_P3P) # Also played around with other methods, but that's not the problem here

        # If found a solution: project the marker's XYZ vectors and display them
        if success:
            points, jacobian = cv2.projectPoints(axis, rotation_vector,
                                                 translation_vector,
                                                 calib_matrix, dist_coeffs)

            # Draw everything
            drawAxes(frame, points_2D, points)

Some results

The solvePnP function only works fine when the points_2D order (i.e. the blobs order) strictly matches the expected points_3D order. Here I have all the points numerated and detected in valid order, as it is defined in points_3D (take a look at points_3D definition in the code and compare to the picture, you’ll understand what I mean). As long as the order is valid - axes do match the reality. You can see the marker is kind of a reverse ‘L’ shape containing 4 LEDs.
See the 1st screenshot

As soon as the points_2D (blobs) order doesn’t match the points_3D order - it fails to compute proper axis relation, you can see the lines don’t even match the marker plane.
See the 2nd screenshot

pnp_blob
pnp_blob640×1104 104 KB

Question

Is there any possible solution (hardware, e.g. marker form, or software) which will help solvePnP work with of any blobs order? Some way for it do decide which point is which? Or maybe there is any algorithmic way of sorting every blob the way solvePnP expects?

Something tells me that that might be impossible as you can probably rotate any given 3D points group that way, that there will be more than a single 2D projection of them on a plane… Or, I just might need more LEDs…? :grinning: :thinking:

2

I remember talking about this.

threads:

3

Thank you! I read through both posts and looks like for now the reprojection method is the best solution for me.
So I now read 4 blob points from the image, than create a permutations array with a function from itertools resulting in 24 different order combinations of them (4! = 24) Then for each permutation I’m solving PnP to get rvec and tvec, then use them to project the 3D points array onto 2D image plane and compare the projected points with the ones originally detected as blobs (made an ‘isCloseTo’ function to let the point be offset by some amount of error). So the permutation for which all the points do match is the solution.

However I ran into some problem with permutations. So when I first displayed all 24 permutations resulting in 24 * 4 points on the screen – that was a mess, but I was quite happy to see that the majority of them were creating a big plane which was perfectly reacting to the marker rotation and movement.
I was a bit fooled with the fact that on each LED position there was always a reprojected point visible, meaning there’s always a solution I only need to find – but that’s not so, as basically some of the wrong solutions were taking place at LEDs making the marker all covered in points, but from different solutions.

I wrote a functional code that should be working fine, but again, it only does work for the easy case when all the points are in the valid order from the beginning. So I’m investigating.
For now I can see two possible problems:

  1. Something is wrong with permutations, so there’s no valid point order generated to solve PnP at all;
  2. Solve PnP is failing to get proper values even for the valid order. I tried to display all the 24*4 points and grouped each reprojection by different color. So each solution had a different color. It was hard to see but I didn’t see a combination that was projecting all 4 points on LEDs with the same color. So maybe there was no valid solution, or I just couldn’t see it because some points may overlap each other perfectly and the ones under can’t be seen.
    Also, maybe solvePnP has some limited range of points rotation? E.g. if the 2D rotation is too big like more than 180 deg clockwise then it’s cannot be solved for the valid order either? But I really doubt about this…

So I’m still playing around. First of all I’ll check that there is a valid permutation generated, also maybe play around with the marker form, but I guess that’s not the problem…

I would like to hear if you have any ideas! Thank you for the two topics you noted, that was quite helpful!

4

Yes! It works!

First of all I looked through the generated permutations list and verified there was a valid points permutation generated. Then I took its index and verified that the solvePnP result and therefore the projectedPoints were valid as well! So that was just two small bugs in the code:

  1. My ‘isCloseTo’ function was only verifying the X point coord, but not Y ( :man_facepalming:x1);
  2. I was validating the projectedPoints with the original ones, but not with permutation I was solving PnP for (:man_facepalming:x2).

After fixing this two issues everything worked!

Thank you so much for your help, again! Here is a small demo on how it looks. The Z axis (blue) is flipping sometimes because the marker is flat i.e. you can rotate it 180 deg along Y axis and it will still be the same shape.

ir-tracking