C++ MapPoint::RefreshPixelVectors方法代码示例

//  Adds map points at a given initial depth in a specified pyramid level
void MapMakerServerBase::AddInitDepthMapPoints(MultiKeyFrame& mkfSrc, int nLevel, int nLimit, double dInitDepth)
{
  for (KeyFramePtrMap::iterator it = mkfSrc.mmpKeyFrames.begin(); it != mkfSrc.mmpKeyFrames.end(); it++)
  {
    KeyFrame& kfSrc = *(it->second);
    TaylorCamera& cameraSrc = mmCameraModels[kfSrc.mCamName];
    Level& level = kfSrc.maLevels[nLevel];
    ThinCandidates(kfSrc, nLevel);

    int nLevelScale = LevelScale(nLevel);

    std::cout << "AddInitDepthMapPoints, processing " << level.vCandidates.size() << " candidates" << std::endl;

    for (unsigned int i = 0; i < level.vCandidates.size() && static_cast<int>(i) < nLimit; ++i)
    {
      TooN::Vector<2> v2RootPos = LevelZeroPos(level.vCandidates[i].irLevelPos, nLevel);
      TooN::Vector<3> v3UnProj = cameraSrc.UnProject(v2RootPos) *
                           dInitDepth;  // This unprojects the noisy image location to a depth of dInitDepth

      MapPoint* pPointNew = new MapPoint;
      pPointNew->mv3WorldPos = kfSrc.mse3CamFromWorld.inverse() * v3UnProj;
      pPointNew->mpPatchSourceKF = &kfSrc;
      pPointNew->mbFixed = false;
      pPointNew->mnSourceLevel = nLevel;
      pPointNew->mv3Normal_NC = TooN::makeVector(0, 0, -1);
      pPointNew->mirCenter = level.vCandidates[i].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();

      mMap.mlpPoints.push_back(pPointNew);

      Measurement* pMeas = new Measurement;
      pMeas->v2RootPos = v2RootPos;
      pMeas->nLevel = nLevel;
      pMeas->eSource = Measurement::SRC_ROOT;

      kfSrc.AddMeasurement(pPointNew, pMeas);
      // kfSrc.mmpMeasurements[pPointNew] = pMeas;
      // pPointNew->mMMData.spMeasurementKFs.insert(&kfSrc);
    }
  }
}

//.........这里部分代码省略.........
    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;

// Initialize the map
bool MapMakerCalib::InitFromCalibImage(CalibImageTaylor& calibImage, double dSquareSize, std::string cameraName,
                                       TooN::SE3<>& se3TrackerPose)
{
    // Create a new MKF
    MultiKeyFrame* pMKF = new MultiKeyFrame;

    pMKF->mse3BaseFromWorld = calibImage.mse3CamFromWorld;     // set mkf pose to be pose from tracker
    pMKF->mse3BaseFromWorld.get_translation() *= dSquareSize;  // scale it
    pMKF->mbFixed = false;

    // Create a new KF (there will only be one for now)
    KeyFrame* pKF = new KeyFrame(pMKF, cameraName);
    pMKF->mmpKeyFrames[cameraName] = pKF;

    pKF->mse3CamFromWorld = pMKF->mse3BaseFromWorld;  // same as parent MKF
    pKF->mse3CamFromBase = TooN::SE3<>();                   // set relative pose to identity;
    pKF->mbActive = true;

    pKF->MakeKeyFrame_Lite(calibImage.mImage, true);
    pKF->MakeKeyFrame_Rest();

    // Create MapPoints where the calib image corners are
    // Testing to see if we should create points at all levels or just level 0
    for (int l = 0; l < LEVELS; ++l)
    {
        if (l >= 1)
            break;

        int nLevelScale = LevelScale(l);

        for (unsigned i = 0; i < calibImage.mvGridCorners.size(); ++i)
        {
            MapPoint* pNewPoint = new MapPoint;
            pNewPoint->mv3WorldPos.slice<0, 2>() = dSquareSize * CVD::vec(calibImage.mvGridCorners[i].mirGridPos);
            pNewPoint->mv3WorldPos[2] = 0.0;  // on z=0 plane
            pNewPoint->mbFixed = true;        // the calibration pattern is fixed
            pNewPoint->mbOptimized = true;    // since it won't move it's already in its optimal location

            // Patch source stuff:
            pNewPoint->mpPatchSourceKF = pKF;
            pNewPoint->mnSourceLevel = l;
            pNewPoint->mv3Normal_NC = TooN::makeVector(0, 0, -1);

            TooN::Vector<2> v2RootPos = calibImage.mvGridCorners[i].mParams.v2Pos;

            // Same code as in MapMakerServerBase::AddPointEpipolar
            pNewPoint->mirCenter = CVD::ir_rounded(LevelNPos(v2RootPos, l));
            pNewPoint->mv3Center_NC = mmCameraModels[cameraName].UnProject(v2RootPos);
            pNewPoint->mv3OneRightFromCenter_NC =
                mmCameraModels[cameraName].UnProject(v2RootPos + CVD::vec(CVD::ImageRef(nLevelScale, 0)));
            pNewPoint->mv3OneDownFromCenter_NC =
                mmCameraModels[cameraName].UnProject(v2RootPos + CVD::vec(CVD::ImageRef(0, nLevelScale)));

            normalize(pNewPoint->mv3Center_NC);
            normalize(pNewPoint->mv3OneDownFromCenter_NC);
            normalize(pNewPoint->mv3OneRightFromCenter_NC);

            pNewPoint->RefreshPixelVectors();

            mMap.mlpPoints.push_back(pNewPoint);

            // Create a measurement of the point
            Measurement* pMeas = new Measurement;
            pMeas->eSource = Measurement::SRC_ROOT;
            pMeas->v2RootPos = v2RootPos;
            pMeas->nLevel = l;
            pMeas->bSubPix = true;
            pKF->mmpMeasurements[pNewPoint] = pMeas;

            pNewPoint->mMMData.spMeasurementKFs.insert(pKF);
        }
    }

    mMap.mlpMultiKeyFrames.push_back(pMKF);

    /*
    {
      ROS_DEBUG("After creating points, before optimization: ");
      int i=0;
      double dErrorSum = 0;
      for(MapPointPtrList::iterator it = mMap.mlpPoints.begin(); it != mMap.mlpPoints.end(); ++it, ++i)
      {
        std::cout<<"Point pos: "<<(*it)->mv3WorldPos;
        TooN::Vector<3> v3Cam = pKF->mse3CamFromWorld * (*it)->mv3WorldPos;
        TooN::Vector<2> v2Image = mmCameraModels[cameraName].Project(v3Cam);
        std::cout<<" Reprojected: "<<v2Image<<" Original: "<<calibImage.mvGridCorners[i].mParams.v2Pos;
        std::cout<<std::endl;

        TooN::Vector<2> v2Error = v2Image - calibImage.mvGridCorners[i].mParams.v2Pos;
        dErrorSum += v2Error * v2Error;
      }

      std::cout<<"Error sum: "<<dErrorSum<<" mean: "<<dErrorSum/i<<std::endl;
    }
    */

    mBundleAdjuster.SetNotConverged();

    int nSanityCounter = 0;
//.........这里部分代码省略.........