Stuck after finding both markers

[shubang93]

I was able to successfully build the code and followed through some of the solutions that other questions answered. At this point, the program is able to find both AruCo markers on the image. But for some reason, it is just stuck there and does not go past it. It keeps printing the location of both markers to screen continuously. The output from the program is shown below. One thing that concerns me is Failed to find match for field 'intensity'. line. I see that intensity filter is being used for the point cloud. But I'm just running ROS Velodyne package VLP16_points.launch file. On Rviz, I am able to see the intensity field under /velodyne_points.

Any help is much appreciated!

NODES
  /
    aruco_mapping (aruco_mapping/aruco_mapping)
    find_transform (lidar_camera_calibration/find_transform)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[aruco_mapping-1]: started with pid [24294]
process[find_transform-2]: started with pid [24296]
[ INFO] [1505776473.556181936]: Calibration file path: /home/shubangsridhar/shub_src/extrinsic_calibration/src/lidar_camera_calibration/conf/ueye-serialnum-4102778863.ini
[ INFO] [1505776473.556269936]: Number of markers: 2
[ INFO] [1505776473.556295936]: Marker Size: 0.226
[ INFO] [1505776473.556308936]: Type of space: plane
[ INFO] [1505776473.556347936]: ROI allowed: 0
[ INFO] [1505776473.556365936]: ROI x-coor: 0
[ INFO] [1505776473.556379936]: ROI y-coor: 0
[ INFO] [1505776473.556398936]: ROI width: 32710
[ INFO] [1505776473.556418936]: ROI height: 4273583
[ INFO] [1505776473.559131936]: Calibration data loaded successfully
Size of the frame: 1600 x 1200
Limits: -4 to 4
Limits: -4 to 4
Limits: -4 to 0
Number of markers: 2
Intensity threshold (between 0.0 and 1.0): 0.02
useCameraInfo: 1
Projection matrix: 
[805.45886, 0, 800.26105, 0;
  0, 805.26483, 603.10297, 0;
  0, 0, 1, 0]
MAX_ITERS: 1
[ INFO] [1505776473.575178936]: Reading CameraInfo from topic
done1
25=(310.637,608.945) (218.48,681.984) (128.23,605.684) (227.578,534.446) Txyz=-1.23591 0.010846 1.72566 Rxyz=-0.379636 1.37063 -1.90313 
582=(899.297,610.282) (838.419,675.12) (771.255,613.907) (832.576,552.011) Txyz=0.0875873 0.0225216 2.00668 Rxyz=-0.804321 1.5226 -2.06702 
[ INFO] [1505776483.425556935]: First marker with ID: 25 detected
[ WARN] [1505776483.429582935]: TF to MSG: Quaternion Not Properly Normalized
[ INFO] [1505776483.491359935]: Camera info received at 1.50578e+09
[ INFO] [1505776483.491411935]: Velodyne scan received at 1.50578e+09
[ INFO] [1505776483.491425936]: marker_6dof received at 1.50578e+09
Failed to find match for field 'intensity'.
25 -1.23591 0.010846 1.72566 -0.379636 1.37063 -1.90313 582 0.0875873 0.0225216 2.00668 -0.804321 1.5226 -2.06702 
[ INFO] [1505776483.493985935]: iteration number: 0

---------Moving on to next marker--------
582=(899.328,610.288) (838.491,675.034) (771.252,613.848) (832.693,552.059) Txyz=0.0877038 0.0224342 2.0062 Rxyz=-0.80573 1.51786 -2.07075 
25=(310.461,608.854) (218.511,681.82) (128.524,605.7) (227.515,534.588) Txyz=-1.23841 0.0108164 1.72915 Rxyz=-0.381727 1.37277 -1.90387 
582=(899.179,610.352) (838.482,675.026) (771.325,613.815) (832.659,552.14) Txyz=0.0877794 0.0225476 2.00906 Rxyz=-0.800976 1.51713 -2.06865 
25=(310.415,608.931) (218.611,681.7) (128.44,605.721) (227.527,534.674) Txyz=-1.24015 0.0108746 1.73166 Rxyz=-0.374344 1.36729 -1.90307 
582=(899.201,610.354) (838.438,674.998) (771.341,613.83) (832.609,551.993) Txyz=0.0877532 0.0224742 2.00929 Rxyz=-0.799631 1.52126 -2.06486 
25=(310.468,608.963) (218.554,681.845) (128.369,605.734) (227.551,534.594) Txyz=-1.23849 0.0109205 1.72927 Rxyz=-0.377402 1.36981 -1.90236 
582=(899.207,610.279) (838.416,675.121) (771.273,613.924) (832.494,551.981) Txyz=0.0875637 0.0225483 2.00808 Rxyz=-0.800433 1.52453 -2.06427 
25=(310.473,608.933) (218.484,681.905) (128.375,605.759) (227.554,534.503) Txyz=-1.23726 0.0108846 1.72747 Rxyz=-0.381968 1.37321 -1.90287 
582=(899.122,610.443) (838.41,675.137) (771.231,613.829) (832.629,552.245) Txyz=0.0875896 0.0227273 2.00819 Rxyz=-0.800913 1.51456 -2.0699 
25=(310.554,608.924) (218.456,681.872) (128.316,605.737) (227.542,534.537) Txyz=-1.2372 0.0108513 1.72742 Rxyz=-0.379603 1.36957 -1.9054 
582=(898.964,610.388) (838.418,675.058) (771.61,613.883) (832.442,552.419) Txyz=0.0879597 0.0229165 2.01592 Rxyz=-0.796683 1.51967 -2.06589 
25=(310.473,608.964) (218.481,681.855) (128.292,605.76) (227.53,534.484) Txyz=-1.23754 0.0108705 1.72781 Rxyz=-0.379375 1.3714 -1.90231 
582=(898.918,610.433) (838.426,675.075) (771.504,613.949) (832.482,552.386) Txyz=0.0878678 0.0229718 2.0153 Rxyz=-0.796319 1.52035 -2.06511 
25=(310.593,608.907) (218.481,681.826) (128.269,605.749) (227.561,534.504) Txyz=-1.23702 0.010798 1.72721 Rxyz=-0.378176 1.36781 -1.90667 
582=(898.723,610.276) (838.547,675.061) (771.739,614.017) (832.356,552.432) Txyz=0.0881584 0.0230551 2.0211 Rxyz=-0.788952 1.52711 -2.05927 
25=(310.568,608.95) (218.42,681.895) (128.269,605.755) (227.593,534.474) Txyz=-1.23647 0.0108444 1.72637 Rxyz=-0.38093 1.37005 -1.90567

[shubang93]

I was able to change things around and get it work. After digging into the code, these are some of the assumptions that are made. Please confirm or deny them so that I can understand better how it works:

1. point_cloud = transform(point_cloud, 0, 0, 0, M_PI/2, -M_PI / 2, 0) line transforms the point cloud into an approximate frame of the camera. This assumes that we know this rotation matrix before hand. And those numbers are hard coded for the test setup discussed in the README.
2. Given that the point cloud is rotated before the transformation is found, there is no correction for the fact that it point cloud was transformed prior to this. The transformation matrix and information that is printed is based on the transformed point cloud mentioned in 1.
3. The X, Y and Z limits mentioned in the config file are not being used. When filtering point cloud for view, the code assumed point cloud is in camera frame. For Z filtering, it only checks if Z is positive in the project() method. For X, Y it just checks if it is in the FOV of the camera and includes it.

[ankitdhall]

1. Yes, that is to transform the point cloud to the frame of the camera.
2. Yes.
3. The X, Y, Z limits in the file are to truncate points that do not satisfy the limits in X, Y and Z. These values are in meters and are used to display the points on the image plane when you are asked to mark the polygons. The points that you see are the ones that have satisfied the conditions for X, Y and Z. These can be adjusted as per your requirements.

[KleinYuan]

@shubang93 Hey, how did you make it work? Met the same situation now. It keeps printing Txyz and Rxyz, hanging at -----Moving to next marker----.

[shubang93]

@KleinYuan I don't quite remember. I realized that the printing of those transforms is just a debug statement. So I commented those out. Also go through the list of assumptions I listed above to make sure you meet them. If your setup is in any way different than what this code is setup for, it will fail and you will see a black image. Also, I'm not convinced that the X,Y,Z limits in the config file are actually used (but I may be wrong).

[KleinYuan]

@shubang93 After reading this project line by line, I finally realize where's the problem. Thanks tho.

[blutjens]

@KleinYuan Could you please share, how you made it work or what the key points of understanding have been? I am stuck at the same point and I'm displayed a black 'cloud' image. Thanks very much!

[blutjens]

I've figured it out. The 'cloud' window displays the markers now. The issue was, that I've changed the transformations in config_file.txt. Changing them back to the default values and assuring that the markers are placed directly in front of the lidar has fixed the problem.

fx 0 cx 0 0 fy cy 0 0 0 1 0 MAX_ITERS initial_rot_x initial_rot_y initial_rot_z

[KleinYuan]

@blutjens I think what blocked me is how to use this program. When two windows poped up, you are supposed to use your mouse to interface, inputting the polygon.

[PanZhichen]

@shubang93 @karnikram @blutjens hello, I am attempting to calibrate lidar and monocular use this pkg, but I am stuck at the same situation as you mentioned, and I still cannot work it out. So can you send me a copy of your custom version of this pkg (email:zhchpan2017@gmail.com)? Thanks very much for your help!

[blutjens]

@PanZhichen

1. Try approach from #26
2. Make sure the 'cloud' window displays the markers as white velodyne points. If the window is black, it could be for two reasons: 2.1. Your markers are not in the given bounding box. -> Change parameters in config_file.txt:

   - 3.5 1.0 # horizontal (x) ROI to find pointclouds. Relative to camera position and coordinate frame
   - 3.0 3.0 # vertical (y), negative direction is top, positive direction is into bottom
   0.0 2.5 # depth (z), negative direction is back of camera, positive is front 

   2.2. The relative rotation of the velodyne to the camera (base node) is off. -> Change parameters in config_file.txt 1.57 1.55 0 The second value rotates parallel to the ground plane. Decrease the value to shift the 'cloud' FOV to the left. Increase to shift to the right.

3. If 'cloud' displays the white markers, 'polygon' will display the markers in blue or just display a black window. Either way, start inputting bounding boxes for the edges in the 'cloud' window. Therefore, follow the provided tutorial on youtube by ankit dhall. To input a bounding box, click on a pixel in the 'cloud' window with the mouse, than press a keyboard button and repeat that process for every corner.

Hope that helps!