fiducial_slam pose estimation adds random aruco markers

[jack-digilabs]

I'm using the latest kinetic-devel branch of fiducials and when I use fiducial_slam pose estimation I get random Aruco markers added to the map even when I have aruco_detect ignoring all Aruco markers I'm not using. Attached is a zip folder containing aruco_detect.launch and fiducial_slam.launch I'm using and the map fiducial_slam created. I only have aruco markers 1,2, and 3 in view of the camera and the camera is stationary. I see the variances are quite high on the randomly "detected" (quotes because I think fiducial_pose node is reading the frame_id instead of fiducial_id but I can't find where) so they won't contribute much to the pose estimate but I think it's still a concern.

launch.zip
map.txt

The erroneous markers do not appear if I enable the rosparam do_pose_estimation in aruco_detect node and disable the rosparam do_pose_estimation in fiducial_slam.

Let me know if there's anything else you need to help find the issue.

[jim-v]

It seems more likley that aruco_detect is detecting the erroneous fiducials. Note that the dictionary you use gives a trade off between better error correction, or a large number of distinct fiducials.

See the "Selecting a Dictionary" section of https://docs.opencv.org/3.4.2/d5/dae/tutorial_aruco_detection.html.

If you want to be certain that the fiducials are coming from aruco_detect and not being generated by fiducial_slam, look at the fiducial_transforms topic.

[jack-digilabs]

Ah you are right. I thought I isolated the erroneous detections to fiducial_slam but I just checked the aruco_detect code and found...
if (doPoseEstimation) {
... stuff ...
if (std::count(ignoreIds.begin(), ignoreIds.end(), ids[i]) != 0) {
ROS_INFO("Ignoring id %d", ids[i]);
continue;
}
... stuff ...
}

So the rosparam ignore_fiducials is only used when the do_pose_estimation is true.

[jim-v]

Thanks for pointing that out. That is a bug, it was intended that the ignore_fiducials parameter would be independent of pose estimation.

[jack-digilabs]

The code below seems to be working for me in that I have multiple warnings that ID's were detected and ignored and it hasn't polluted the fiducial_slam map.

    for (size_t i=0; i<ids.size(); i++) {
        if (std::count(ignoreIds.begin(), ignoreIds.end(), ids[i]) != 0) {
            ROS_WARN("Ignoring id %d", ids[i]);
            continue;
        }
        fiducial_msgs::Fiducial fid;
        fid.fiducial_id = ids[i];

        fid.x0 = corners[i][0].x;
        fid.y0 = corners[i][0].y;
        fid.x1 = corners[i][1].x;
        fid.y1 = corners[i][1].y;
        fid.x2 = corners[i][2].x;
        fid.y2 = corners[i][2].y;
        fid.x3 = corners[i][3].x;
        fid.y3 = corners[i][3].y;
        fva.fiducials.push_back(fid);
    }

[jim-v]

Can you make a pull request, then we can incorporate your fix.

[jack-digilabs]

Submitted #143 to fix this issue.

[jack-digilabs]

#143 fixes this issue.