// Mapmaker's try-to-find-a-point-in-a-keyframe code. This is used to update
// data association if a bad measurement was detected, or if a point
// was never searched for in a keyframe in the first place. This operates
// much like the tracker! So most of the code looks just like in
// TrackerData.h.
bool MapMakerServerBase::ReFind_Common(KeyFrame& kf, MapPoint& point)
{
// abort if either a measurement is already in the map, or we've
// decided that this point-kf combo is beyond redemption
if (point.mMMData.spMeasurementKFs.count(&kf) || point.mMMData.spNeverRetryKFs.count(&kf))
return false;
if (point.mbBad)
return false;
if (kf.mpParent->mbBad)
return false;
// debug
static GVars3::gvar3<int> gvnCrossCamera("CrossCamera", 1, GVars3::HIDDEN | GVars3::SILENT);
if (!*gvnCrossCamera && kf.mCamName != point.mpPatchSourceKF->mCamName)
return false;
static PatchFinder finder;
TooN::Vector<3> v3Cam = kf.mse3CamFromWorld * point.mv3WorldPos;
TaylorCamera& camera = mmCameraModels[kf.mCamName];
TooN::Vector<2> v2Image = camera.Project(v3Cam);
if (camera.Invalid())
{
point.mMMData.spNeverRetryKFs.insert(&kf);
return false;
}
CVD::ImageRef irImageSize = kf.maLevels[0].image.size();
if (v2Image[0] < 0 || v2Image[1] < 0 || v2Image[0] > irImageSize[0] || v2Image[1] > irImageSize[1])
{
point.mMMData.spNeverRetryKFs.insert(&kf);
return false;
}
TooN::Matrix<2> m2CamDerivs = camera.GetProjectionDerivs();
finder.MakeTemplateCoarse(point, kf.mse3CamFromWorld, m2CamDerivs);
if (finder.TemplateBad())
{
point.mMMData.spNeverRetryKFs.insert(&kf);
return false;
}
int nScore;
bool bFound = finder.FindPatchCoarse(CVD::ir(v2Image), kf, 4, nScore); // Very tight search radius!
if (!bFound)
{
point.mMMData.spNeverRetryKFs.insert(&kf);
return false;
}
// If we found something, generate a measurement struct and put it in the map
Measurement* pMeas = new Measurement;
pMeas->nLevel = finder.GetLevel();
pMeas->eSource = Measurement::SRC_REFIND;
if (finder.GetLevel() > 0)
{
finder.MakeSubPixTemplate();
finder.SetSubPixPos(finder.GetCoarsePosAsVector());
finder.IterateSubPixToConvergence(kf, 8);
pMeas->v2RootPos = finder.GetSubPixPos();
pMeas->bSubPix = true;
}
else
{
pMeas->v2RootPos = finder.GetCoarsePosAsVector();
pMeas->bSubPix = false;
}
if (kf.mmpMeasurements.count(&point))
ROS_BREAK(); // This should never happen, we checked for this at the start.
kf.AddMeasurement(&point, pMeas);
// kf.mmpMeasurements[&point] = pMeas;
// point.mMMData.spMeasurementKFs.insert(&kf);
return true;
}
// Tries to make a new map point out of a single candidate point
// by searching for that point in another keyframe, and triangulating
// if a match is found.
bool MapMakerServerBase::AddPointEpipolar(KeyFrame& kfSrc, KeyFrame& kfTarget, int nLevel, int nCandidate)
{
// debug
static GVars3::gvar3<int> gvnCrossCamera("CrossCamera", 1, GVars3::HIDDEN | GVars3::SILENT);
if (!*gvnCrossCamera && kfSrc.mCamName != kfTarget.mCamName)
return false;
TaylorCamera& cameraSrc = mmCameraModels[kfSrc.mCamName];
TaylorCamera& cameraTarget = mmCameraModels[kfTarget.mCamName];
int nLevelScale = LevelScale(nLevel);
Candidate& candidate = kfSrc.maLevels[nLevel].vCandidates[nCandidate];
CVD::ImageRef irLevelPos = candidate.irLevelPos;
TooN::Vector<2> v2RootPos = LevelZeroPos(irLevelPos, nLevel); // The pixel coords of the candidate at level zero
TooN::Vector<3> v3Ray_SC =
cameraSrc.UnProject(v2RootPos); // The pixel coords unprojected into a 3d half-line in the source kf frame
TooN::Vector<3> v3LineDirn_TC =
kfTarget.mse3CamFromWorld.get_rotation() * (kfSrc.mse3CamFromWorld.get_rotation().inverse() *
v3Ray_SC); // The direction of that line in the target kf frame
TooN::Vector<3> v3CamCenter_TC =
kfTarget.mse3CamFromWorld *
kfSrc.mse3CamFromWorld.inverse().get_translation(); // The position of the source kf in the target kf frame
TooN::Vector<3> v3CamCenter_SC =
kfSrc.mse3CamFromWorld *
kfTarget.mse3CamFromWorld.inverse().get_translation(); // The position of the target kf in the source kf frame
double dMaxEpiAngle = M_PI / 3; // the maximum angle spanned by two view rays allowed
double dMinEpiAngle = 0.05; // the minimum angle allowed
// Want to figure out the min and max depths allowed on the source ray, which will be determined by the minimum and
// maximum allowed epipolar angle
// See diagram below, which shows the min and max epipolar angles.
/*
* /\
* / m\
* / i \
* /`. n \
* / m `. \
* / a `. \
* / x `. \
* /____________`.\
* Source Target
*/
double dSeparationDist = norm(v3CamCenter_SC);
double dSourceAngle =
acos((v3CamCenter_SC * v3Ray_SC) / dSeparationDist); // v3Ray_SC is unit length so don't have to divide
double dMinTargetAngle = M_PI - dSourceAngle - dMaxEpiAngle;
double dStartDepth = dSeparationDist * sin(dMinTargetAngle) / sin(dMaxEpiAngle);
double dMaxTargetAngle = M_PI - dSourceAngle - dMinEpiAngle;
double dEndDepth = dSeparationDist * sin(dMaxTargetAngle) / sin(dMinEpiAngle);
if (dStartDepth < 0.2) // don't bother looking too close
dStartDepth = 0.2;
ROS_DEBUG_STREAM("dStartDepth: " << dStartDepth << " dEndDepth: " << dEndDepth);
TooN::Vector<3> v3RayStart_TC =
v3CamCenter_TC + dStartDepth * v3LineDirn_TC; // The start of the epipolar line segment in the target kf frame
TooN::Vector<3> v3RayEnd_TC =
v3CamCenter_TC + dEndDepth * v3LineDirn_TC; // The end of the epipolar line segment in the target kf frame
// Project epipolar line segment start and end points onto unit sphere and check for minimum distance between them
TooN::Vector<3> v3A = v3RayStart_TC;
normalize(v3A);
TooN::Vector<3> v3B = v3RayEnd_TC;
normalize(v3B);
TooN::Vector<3> v3BetweenEndpoints = v3A - v3B;
if (v3BetweenEndpoints * v3BetweenEndpoints < 0.00000001)
{
ROS_DEBUG_STREAM("MapMakerServerBase: v3BetweenEndpoints too small.");
return false;
}
// Now we want to construct a bunch of hypothetical point locations, so we can warp the source patch
// into the target KF and look for a match. To do this, need to partition the epipolar arc in the target
// KF equally, rather than the source ray equally. The epipolar arc lies at the intersection of the epipolar
// plane and the unit circle of the target KF. We will construct a matrix that projects 3-vectors onto
// the epipolar plane, and use it to define the start and stop coordinates of a unit circle by
// projecting the ray start and ray end vectors. Then it's just a matter of partitioning the unit circle, and
// projecting each partition point onto the source ray (keeping in mind that the source ray is in the
// epipolar plane too).
// Find the normal of the epipolar plane
TooN::Vector<3> v3PlaneNormal = v3A ^ v3B;
normalize(v3PlaneNormal);
TooN::Vector<3> v3PlaneI =
v3A; // Lets call the vector we got from the start of the epipolar line segment the new coordinate frame's x axis
TooN::Vector<3> v3PlaneJ = v3PlaneNormal ^ v3PlaneI; // Get the y axis
// This will convert a 3D point to the epipolar plane's coordinate frame
TooN::Matrix<3> m3ToPlaneCoords;
m3ToPlaneCoords[0] = v3PlaneI;
m3ToPlaneCoords[1] = v3PlaneJ;
m3ToPlaneCoords[2] = v3PlaneNormal;
TooN::Vector<2> v2PlaneB = (m3ToPlaneCoords * v3B).slice<0, 2>(); // The vector we got from the end of the epipolar
// line segment, in epipolar plane coordinates
TooN::Vector<2> v2PlaneI = TooN::makeVector(1, 0);
double dMaxAngleAlongCircle =
acos(v2PlaneB * v2PlaneI); // The angle between point B (now a 2D point in the plane) and the x axis
// For stepping along the circle
double dAngleStep = cameraTarget.OnePixelAngle() * LevelScale(nLevel) * 3;
int nSteps = ceil(dMaxAngleAlongCircle / dAngleStep);
dAngleStep = dMaxAngleAlongCircle / nSteps;
TooN::Vector<2> v2RayStartInPlane = (m3ToPlaneCoords * v3RayStart_TC).slice<0, 2>();
TooN::Vector<2> v2RayEndInPlane = (m3ToPlaneCoords * v3RayEnd_TC).slice<0, 2>();
TooN::Vector<2> v2RayDirInPlane = v2RayEndInPlane - v2RayStartInPlane;
normalize(v2RayDirInPlane);
// First in world frame, second in camera frame
std::vector<std::pair<TooN::Vector<3>, TooN::Vector<3>>> vMapPointPositions;
TooN::SE3<> se3WorldFromTargetCam = kfTarget.mse3CamFromWorld.inverse();
for (int i = 0; i < nSteps + 1; ++i) // stepping along circle
{
double dAngle = i * dAngleStep; // current angle
TooN::Vector<2> v2CirclePoint = TooN::makeVector(cos(dAngle), sin(dAngle)); // point on circle
// Backproject onto view ray, this is the depth along view ray where we intersect
double dAlpha = (v2RayStartInPlane[0] * v2CirclePoint[1] - v2RayStartInPlane[1] * v2CirclePoint[0]) /
(v2RayDirInPlane[1] * v2CirclePoint[0] - v2RayDirInPlane[0] * v2CirclePoint[1]);
TooN::Vector<3> v3PointPos_TC = v3RayStart_TC + dAlpha * v3LineDirn_TC;
TooN::Vector<3> v3PointPos = se3WorldFromTargetCam * v3PointPos_TC;
vMapPointPositions.push_back(std::make_pair(v3PointPos, v3PointPos_TC));
}
// This will be the map point that we place at the different depths in order to generate warped patches
MapPoint point;
point.mpPatchSourceKF = &kfSrc;
point.mnSourceLevel = nLevel;
point.mv3Normal_NC = TooN::makeVector(0, 0, -1);
point.mirCenter = irLevelPos;
point.mv3Center_NC = cameraSrc.UnProject(v2RootPos);
point.mv3OneRightFromCenter_NC = cameraSrc.UnProject(v2RootPos + vec(CVD::ImageRef(nLevelScale, 0)));
point.mv3OneDownFromCenter_NC = cameraSrc.UnProject(v2RootPos + vec(CVD::ImageRef(0, nLevelScale)));
normalize(point.mv3Center_NC);
normalize(point.mv3OneRightFromCenter_NC);
normalize(point.mv3OneDownFromCenter_NC);
PatchFinder finder;
int nMaxZMSSD = finder.mnMaxSSD + 1;
int nBestZMSSD = nMaxZMSSD;
int nBest = -1;
TooN::Vector<2> v2BestMatch = TooN::Zeros;
std::vector<std::tuple<int, int, TooN::Vector<2>>> vScoresIndicesBestMatches;
for (unsigned i = 0; i < vMapPointPositions.size(); ++i) // go through all our hypothesized map points
{
point.mv3WorldPos = vMapPointPositions[i].first;
point.RefreshPixelVectors();
TooN::Vector<2> v2Image = cameraTarget.Project(vMapPointPositions[i].second);
if (cameraTarget.Invalid())
continue;
if (!kfTarget.maLevels[0].image.in_image(CVD::ir(v2Image)))
continue;
// Check if projected point is in a masked portion of the target keyframe
if (kfTarget.maLevels[0].mask.totalsize() > 0 && kfTarget.maLevels[0].mask[CVD::ir(v2Image)] == 0)
continue;
TooN::Matrix<2> m2CamDerivs = cameraTarget.GetProjectionDerivs();
int nSearchLevel = finder.CalcSearchLevelAndWarpMatrix(point, kfTarget.mse3CamFromWorld, m2CamDerivs);
if (nSearchLevel == -1)
continue;
finder.MakeTemplateCoarseCont(point);
if (finder.TemplateBad())
continue;
int nScore;
bool bExhaustive =
false; // Should we do an exhaustive search of the target area? Should maybe make this into a param
bool bFound = finder.FindPatchCoarse(CVD::ir(v2Image), kfTarget, 3, nScore, bExhaustive);
if (!bFound)
continue;
vScoresIndicesBestMatches.push_back(std::make_tuple(nScore, i, finder.GetCoarsePosAsVector()));
if (nScore < nBestZMSSD)
{
nBestZMSSD = nScore;
nBest = i;
v2BestMatch = finder.GetCoarsePosAsVector();
}
}
if (nBest == -1)
{
ROS_DEBUG_STREAM("No match found.");
return false;
}
std::sort(vScoresIndicesBestMatches.begin(), vScoresIndicesBestMatches.end(), compScores);
// We want matches that are unambigous, so if there are many good matches along the view ray,
// we can't say for certain where the best one really is. Therefore, implement the following rule:
// Best zmssd has to be 10% better than nearest other, unless that nearest other is 1 index away
// from best
int nResizeTo = 1;
for (unsigned i = 1; i < vScoresIndicesBestMatches.size(); ++i)
{
if (std::get<0>(vScoresIndicesBestMatches[i]) > nBestZMSSD * 0.9) // within 10% of best
nResizeTo++;
}
// Too many high scoring points, since the best can be within 10% of at most two other points.
// We can't be certain of what is best, get out of here
if (nResizeTo > 3)
return false;
vScoresIndicesBestMatches.resize(nResizeTo); // chop!
// All the points left in vScoresIndicesBestMatches should be within 1 idx of best, otherwise our matches are ambigous
// Test index distance:
for (unsigned i = 1; i < vScoresIndicesBestMatches.size(); ++i)
{
if (abs(std::get<1>(vScoresIndicesBestMatches[i]) - nBest) > 1) // bad, index too far away, get out of here
return false;
}
// Now all the points in vScoresIndicesBestMatches can be considered potential matches
TooN::Vector<2> v2SubPixPos = TooN::makeVector(-1, -1);
bool bGotGoodSubpix = false;
for (unsigned i = 0; i < vScoresIndicesBestMatches.size(); ++i) // go through all potential good matches
{
int nCurrBest = std::get<1>(vScoresIndicesBestMatches[i]);
TooN::Vector<2> v2CurrBestMatch = std::get<2>(vScoresIndicesBestMatches[i]);
point.mv3WorldPos = vMapPointPositions[nCurrBest].first;
point.RefreshPixelVectors();
cameraTarget.Project(vMapPointPositions[nCurrBest].second);
TooN::Matrix<2> m2CamDerivs = cameraTarget.GetProjectionDerivs();
finder.CalcSearchLevelAndWarpMatrix(point, kfTarget.mse3CamFromWorld, m2CamDerivs);
finder.MakeTemplateCoarseCont(point);
finder.SetSubPixPos(v2CurrBestMatch);
// Try to get subpixel convergence
bool bSubPixConverges = finder.IterateSubPixToConvergence(kfTarget, 10);
if (!bSubPixConverges)
continue;
// First one to make it here wins. Keep in mind that vScoresIndicesBestMatches is ordered by
// score, so we're trying the points in descent order of potential
bGotGoodSubpix = true;
v2SubPixPos = finder.GetSubPixPos();
break;
}
// None of the candidates had subpix converge? Bad match...
if (!bGotGoodSubpix)
return false;
// Now triangulate the 3d point...
TooN::Vector<3> v3New;
v3New = kfTarget.mse3CamFromWorld.inverse() *
ReprojectPoint(kfSrc.mse3CamFromWorld * kfTarget.mse3CamFromWorld.inverse(), cameraSrc.UnProject(v2RootPos),
cameraTarget.UnProject(v2SubPixPos));
MapPoint* pPointNew = new MapPoint;
pPointNew->mv3WorldPos = v3New;
// Patch source stuff:
pPointNew->mpPatchSourceKF = &kfSrc;
pPointNew->mnSourceLevel = nLevel;
pPointNew->mv3Normal_NC = TooN::makeVector(0, 0, -1);
pPointNew->mirCenter = irLevelPos;
pPointNew->mv3Center_NC = cameraSrc.UnProject(v2RootPos);
pPointNew->mv3OneRightFromCenter_NC = cameraSrc.UnProject(v2RootPos + vec(CVD::ImageRef(nLevelScale, 0)));
pPointNew->mv3OneDownFromCenter_NC = cameraSrc.UnProject(v2RootPos + vec(CVD::ImageRef(0, nLevelScale)));
normalize(pPointNew->mv3Center_NC);
normalize(pPointNew->mv3OneDownFromCenter_NC);
normalize(pPointNew->mv3OneRightFromCenter_NC);
pPointNew->RefreshPixelVectors();
Measurement* pMeasSrc = new Measurement;
pMeasSrc->eSource = Measurement::SRC_ROOT;
pMeasSrc->v2RootPos = v2RootPos;
pMeasSrc->nLevel = nLevel;
pMeasSrc->bSubPix = true;
Measurement* pMeasTarget = new Measurement;
*pMeasTarget = *pMeasSrc; // copy data
pMeasTarget->eSource = Measurement::SRC_EPIPOLAR;
pMeasTarget->v2RootPos = v2SubPixPos;
// Record map point and its measurement in the right places
kfSrc.AddMeasurement(pPointNew, pMeasSrc);
kfTarget.AddMeasurement(pPointNew, pMeasTarget);
// kfSrc.mmpMeasurements[pPointNew] = pMeasSrc;
// kfTarget.mmpMeasurements[pPointNew] = pMeasTarget;
// pPointNew->mMMData.spMeasurementKFs.insert(&kfSrc);
// pPointNew->mMMData.spMeasurementKFs.insert(&kfTarget);
mMap.mlpPoints.push_back(pPointNew);
mlpNewQueue.push_back(pPointNew);
return true;
}
