Revision history [back]

Well, objectPointsTri has a 1<->1 mapping with descCurrent. And, matchPoints is (query, train), if you didn't reverse the parameters. So that means the trainIdx property of the match has the index in both descCurrent and objectPointsTri.

for(cv::DMatch& m : match)
{
    cv::Point3f pos = objectPointsTri[m.trainIdx];
    //Do stuff with it
}

for(cv::DMatch& m : match)
{
    cv::Point3f pos = objectPointsTri[m.trainIdx];
    //Do stuff with it
}

EDIT:

Ok, to understand what you're doing, you need to break this apart into functions. You have one long block and that's probably part of why you're having trouble.

The functions you need that I'm not going to write for you are (and you already have some of these)

DetectKeypoints (Takes image, returns keypoints and descriptors)
MatchKeypoints (Takes descriptors, returns matches) Below I've used 1st arg as query, 2nd as train.
FilterMatches (Takes matches, takes either keypoints or descriptors, takes indication if query or train, returns filtered list)

Adjust3dPoints (Takes 3d points, rvec, tvec, returns the points in absolute coordinate frame

if(firstFrame)
{
    kpsL, descL = DetectKeypoints(left)
    kpsR, descR = DetectKeypoints(right)
    matches = MatchKeypoints(descL, descR)
    goodL = FilterMatches(kpsL, matches, query)
    goodD = FilterMatches(descL, matches, query)
    goodR = FilterMatches(kpsR, matches, train)
    3dPts = Triangulate(goodL, goodR)
}
else
{
    kpsL, descL = DetectKeypoints(left)
    kpsR, descR = DetectKeypoints(right)
    matches = MatchKeypoints(goodD, descL)
    3dPts = FilterMatches(3dPts, query)
    goodNew = FilterMatches(kpsL, train)
    rvec, tvec = solvePnP(3dPts, goodNew)

    matches = MatchKeypoints(descL, descR)
    goodL = FilterMatches(kpsL, matches, query)
    goodD = FilterMatches(descL, matches, query)
    goodR = FilterMatches(kpsR, matches, train)
    3dPts = Triangulate(goodL, goodR)
    3dPts = Adjust3dPoints(3dPts, rvec, tvec)
}

So each set of frames produces a list of keypoints for L an R, one for each match, in order, A matching set of Descriptors for L, and 3dPts, one for each match, in the same order.

Everything after the first matches the new L to the old L, and filters by those matches. Then those get used for solvePnp.

After the first frame you have to adjust the 3dPoints, because triangulate assumes the camera is the coordinate reference, but we know it isn't, so you have to adjust for that.