C++ MapPoint类代码示例

void NewMapPtsNCC::output() {
	decidePointType();
	for (size_t i = 0; i < newMapPts.size(); i++) {
		MapPoint* mpt = newMapPts[i];
		for (int c = 0; c < numCams; c++) {
			int camId = m_camGroup.camIds[c];
			if (mpt->pFeatures[camId]) {
				for (FeaturePoint* fp = mpt->pFeatures[camId]; fp->nextFrame;
						fp = fp->nextFrame) {
					fp->mpt = mpt;
					mpt->lastFrame =
							fp->f > mpt->lastFrame ? fp->f : mpt->lastFrame;

					mpt->nccBlks[camId].computeScaled(
							pCoSLAM->slam[camId].m_smallImg,
							pCoSLAM->slam[camId].m_smallScale, fp->x, fp->y);

					int x = max(0, min((int) fp->x, pCoSLAM->slam[camId].m_img.w));
					int y = max(0, min((int) fp->y, pCoSLAM->slam[camId].m_img.h));

					mpt->setColor(pCoSLAM->slam[camId].m_rgb(x,y));
				}
			}
		}
		if (newMapPts[i]->lastFrame < pCoSLAM->curFrame) {
			pCoSLAM->actMapPts.add(mpt);
		} else
			pCoSLAM->curMapPts.add(mpt);
	}
}

void SingleSLAM::propagateFeatureStates() {
	for (int i = 0; i < m_tracker.m_nMaxCorners; i++) {
		Track2D& tk = m_tracker.m_tks[i];
		if (tk.empty())
			continue;
		Track2DNode* node = tk.tail;
		if (node->pre) {
			//propagate the type of feature points
			node->pt->type = node->pre->pt->type;
			if (node->pre->pt->mpt) {
				MapPoint* pMapPt = node->pre->pt->mpt;
				if (pMapPt->state != STATE_MAPPOINT_CURRENT)
					continue;
				//map correspondence propagation
				if (!pMapPt->isFalse()) {
					pMapPt->pFeatures[camId] = node->pt;
					node->pt->mpt = pMapPt;
					pMapPt->lastFrame = currentFrame();
					if (pMapPt->isCertainStatic())
						pMapPt->staticFrameNum++;
					node->pt->reprojErr = node->pre->pt->reprojErr;
				}
			}
		}
	}
}

int NewMapPtsNCC::getSeedsBetween(MapPointList& mapPts, int iCam, int jCam,
		Mat_d& seed1, Mat_d& seed2) {
	MapPoint* pHead = mapPts.getHead();
	if (!pHead)
		repErr("SLAM failed - No map point can be found!\n");
	std::vector<FeaturePoint*> vecFeatPts1, vecFeatPts2;
	for (MapPoint* p = pHead; p; p = p->next) {
		//if ((p->iLocalType == TYPE_MAPPOINT_STATIC || p->iLocalType == TYPE_MAPPOINT_DYNAMIC) && p->numVisCam >= 2 && p->pFeatures[iCam] && p->pFeatures[iCam]->f == m_curFrame && p->pFeatures[jCam] && p->pFeatures[jCam]->f == m_curFrame) {
		if (!p->isFalse() && !p->isUncertain() && p->numVisCam >= 2
				&& p->pFeatures[iCam] && p->pFeatures[iCam]->f == m_curFrame
				&& p->pFeatures[jCam] && p->pFeatures[jCam]->f == m_curFrame) {
			vecFeatPts1.push_back(p->pFeatures[iCam]);
			vecFeatPts2.push_back(p->pFeatures[jCam]);
		}
	}
	if (vecFeatPts1.empty())
		return 0;

	seed1.resize(vecFeatPts1.size(), 2);
	seed2.resize(vecFeatPts2.size(), 2);

	for (size_t i = 0; i < vecFeatPts1.size(); i++) {
		seed1.data[2 * i] = vecFeatPts1[i]->x;
		seed1.data[2 * i + 1] = vecFeatPts1[i]->y;
		seed2.data[2 * i] = vecFeatPts2[i]->x;
		seed2.data[2 * i + 1] = vecFeatPts2[i]->y;
	}
	return seed1.rows;
}

void World::FindContinentNeighbors()
{
    sint16 center_cont;
    MapPoint center;
    MapPoint test_cont;
    bool is_land = false;

    AllocateNeighborMem();

    for(center.x=0; center.x<m_size.x; center.x++) {
        for(center.y=0; center.y<m_size.y; center.y++) {

            GetContinent(center, center_cont, is_land);

            Assert(0 <= center_cont);
            if (center_cont < 0) continue;

            if(center.GetNeighborPosition(NORTHWEST, test_cont))
				FindAContinentNeighbor(center_cont, test_cont, is_land);

            if(center.GetNeighborPosition(WEST, test_cont))
				FindAContinentNeighbor(center_cont, test_cont, is_land);

            if(center.GetNeighborPosition(SOUTHWEST, test_cont))
				FindAContinentNeighbor(center_cont, test_cont, is_land);

            if(center.GetNeighborPosition(SOUTH, test_cont))
				FindAContinentNeighbor(center_cont, test_cont, is_land);
        }
    }
}

int PointRegister::process(double pixelErrVar) {
	/*==============(critical section)=================*/
	enterBACriticalSection();
	bool empty = slam.actMapPts.empty();
	leaveBACriticalSection();
	if (empty)
		return 0;
	/*------------------------------------------------*/
	vector<MapPoint*> vecActMapPts;
	vecActMapPts.reserve(slam.actMapPts.size() * 2);

	/*==============(critical section)=================*/
	enterBACriticalSection();
	typedef std::list<MapPoint*>::iterator MapPointListIter;
	for (MapPointListIter iter = slam.actMapPts.begin();
			iter != slam.actMapPts.end(); iter++) {
		MapPoint* p = *iter;
		vecActMapPts.push_back(p);
	}
	slam.actMapPts.clear();
	slam.nActMapPts = (int) vecActMapPts.size();
	leaveBACriticalSection();
	/*------------------------------------------------*/

	/*get the mask and unmapped feature points for registration*/
	genMappedMask();
	getUnMappedFeatPoints();

	vector<bool> flagReged;
	flagReged.resize((size_t) slam.nActMapPts, false);
	/*==============(critical section)=================*/
	enterBACriticalSection();
	nRegTried = 0;
	nRegDeepTried = 0;
	for (int i = 0; i < slam.nActMapPts; i++) {
		MapPoint* p = vecActMapPts[i];
		if (!p->isFalse()) {
			flagReged[i] = regOnePoint(p, pixelErrVar);
		}
	}
	leaveBACriticalSection();
	/*------------------------------------------------*/

	/*==============(critical section)=================*/
	enterBACriticalSection();
	int nReg = 0;
	for (size_t i = 0; i < slam.nActMapPts; i++) {
		MapPoint* p = vecActMapPts[i];
		if (flagReged[i] == false) {
			slam.actMapPts.push_back(p);
		} else {
			p->lastFrame = slam.curFrame;
			p->state = STATE_MAPPOINT_CURRENT;
			slam.curMapPts.push_back(p);
			nReg++;
		}
	}
	leaveBACriticalSection();
	return nReg;
}

 float KeyFrame::ComputeSceneMedianDepth(int q)
 {
     vector<MapPoint*> vpMapPoints;
     cv::Mat Tcw_;
     {
         boost::mutex::scoped_lock lock(mMutexFeatures);
         boost::mutex::scoped_lock lock2(mMutexPose);
         vpMapPoints = mvpMapPoints;
         Tcw_ = Tcw.clone();
     }
     
     vector<float> vDepths;
     vDepths.reserve(mvpMapPoints.size());
     cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3);
     Rcw2 = Rcw2.t();
     float zcw = Tcw_.at<float>(2,3);
     for(size_t i=0; i<mvpMapPoints.size(); i++)
     {
         if(mvpMapPoints[i])
         {
             MapPoint* pMP = mvpMapPoints[i];
             cv::Mat x3Dw = pMP->GetWorldPos();
             float z = Rcw2.dot(x3Dw)+zcw;
             vDepths.push_back(z);
         }
     }
     
     sort(vDepths.begin(),vDepths.end());
     
     return vDepths[(vDepths.size()-1)/q];
 }

 void LocalMapping::KeyFrameCulling()
 {
     // Check redundant keyframes (only local keyframes)
     // A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen
     // in at least other 3 keyframes (in the same or finer scale)
     vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
     
     for(vector<KeyFrame*>::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++)
     {
         KeyFrame* pKF = *vit;
         if(pKF->mnId==0)
             continue;
         vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
         
         int nRedundantObservations=0;
         int nMPs=0;
         for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
         {
             MapPoint* pMP = vpMapPoints[i];
             if(pMP)
             {
                 if(!pMP->isBad())
                 {
                     nMPs++;
                     if(pMP->Observations()>3)
                     {
                         int scaleLevel = pKF->GetKeyPointUn(i).octave;
                         map<KeyFrame*, size_t> observations = pMP->GetObservations();
                         int nObs=0;
                         for(map<KeyFrame*, size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
                         {
                             KeyFrame* pKFi = mit->first;
                             if(pKFi==pKF)
                                 continue;
                             int scaleLeveli = pKFi->GetKeyPointUn(mit->second).octave;
                             if(scaleLeveli<=scaleLevel+1)
                             {
                                 nObs++;
                                 if(nObs>=3)
                                     break;
                             }
                         }
                         if(nObs>=3)
                         {
                             nRedundantObservations++;
                         }
                     }
                 }
             }
         }
         
         if(nRedundantObservations>0.9*nMPs)
             pKF->SetBadFlag();
     }
 }

 void LocalMapping::ProcessNewKeyFrame()
 {
     {
         boost::mutex::scoped_lock lock(mMutexNewKFs);
         mpCurrentKeyFrame = mlNewKeyFrames.front();
         mlNewKeyFrames.pop_front();
     }
     
     // Compute Bags of Words structures
     mpCurrentKeyFrame->ComputeBoW();
     
     if(mpCurrentKeyFrame->mnId==0)
         return;
     
     // Associate MapPoints to the new keyframe and update normal and descriptor
     vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
     if(mpCurrentKeyFrame->mnId>1) //This operations are already done in the tracking for the first two keyframes
     {
         for(size_t i=0; i<vpMapPointMatches.size(); i++)
         {
             MapPoint* pMP = vpMapPointMatches[i];
             if(pMP)
             {
                 if(!pMP->isBad())
                 {
                     pMP->AddObservation(mpCurrentKeyFrame, i);
                     pMP->UpdateNormalAndDepth();
                     pMP->ComputeDistinctiveDescriptors();
                 }
             }
         }
     }
     
     if(mpCurrentKeyFrame->mnId==1)
     {
         for(size_t i=0; i<vpMapPointMatches.size(); i++)
         {
             MapPoint* pMP = vpMapPointMatches[i];
             if(pMP)
             {
                 mlpRecentAddedMapPoints.push_back(pMP);
             }
         }
     }
     
     // Update links in the Covisibility Graph
     mpCurrentKeyFrame->UpdateConnections();
     
     // Insert Keyframe in Map
     mpMap->AddKeyFrame(mpCurrentKeyFrame);
 }

//  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);
    }
  }
}

void World::GrowLand(MapPoint const & start)
{
    MapPointNode * search_list   = NULL;
    AddToLandSearch(search_list, start);

    MapPoint       test_pos;
    MapPoint       center;
    MapPointNode * finished_list = NULL;
#ifdef _DEBUG
    sint32         finite_loop   = 0;
#endif

    while (NextPoint(search_list, finished_list, center))
    {
		Assert(++finite_loop < 100000);

		if(center.GetNeighborPosition(NORTH, test_pos))
			AddToLandSearch(search_list, test_pos);
		if(center.GetNeighborPosition(NORTHEAST, test_pos))
			AddToLandSearch(search_list, test_pos);
		if(center.GetNeighborPosition(NORTHWEST, test_pos))
			AddToLandSearch(search_list, test_pos);

		if(center.GetNeighborPosition(EAST, test_pos))
			AddToLandSearch(search_list, test_pos);
		if(center.GetNeighborPosition(WEST, test_pos))
			AddToLandSearch(search_list, test_pos);

		if(center.GetNeighborPosition(SOUTH, test_pos))
			AddToLandSearch(search_list, test_pos);
		if(center.GetNeighborPosition(SOUTHEAST, test_pos))
			AddToLandSearch(search_list, test_pos);
		if(center.GetNeighborPosition(SOUTHWEST, test_pos))
			AddToLandSearch(search_list, test_pos);
    }

#ifdef _DEBUG
    finite_loop=0;
#endif

    while (finished_list)
    {
        Assert(++finite_loop < 100000);

        MapPointNode * ptr = finished_list;
        finished_list = finished_list->next;

        GetCell(ptr->pos)->SetContinent(m_land_continent_max);
        delete ptr;
    }

    m_land_continent_max++;
    Assert(LAND_CONTINENT_START < m_land_continent_max);
}

void GameWorldView::MoveToMapPt(const MapPoint pt)
{
    if(!pt.isValid())
        return;

    lastOffset = offset;
    Point<int> nodePos = GetWorld().GetNodePos(pt);

    MoveTo(nodePos - DrawPoint(GetSize() / 2), true);
}

BOOST_FIXTURE_TEST_CASE(HQPlacement, WorldFixtureEmpty1P)
{
    GamePlayer& player = world.GetPlayer(0);
    BOOST_REQUIRE(player.isUsed());
    const MapPoint hqPos = player.GetHQPos();
    BOOST_REQUIRE(hqPos.isValid());
    BOOST_REQUIRE_EQUAL(world.GetNO(player.GetHQPos())->GetGOT(), GOT_NOB_HQ);
    // Check ownership of points
    std::vector<MapPoint> ownerPts = world.GetPointsInRadius(hqPos, HQ_RADIUS);
    BOOST_FOREACH(MapPoint pt, ownerPts)
    {
        // This should be ensured by `GetPointsInRadius`
        BOOST_REQUIRE_LE(world.CalcDistance(pt, hqPos), HQ_RADIUS);
        // We must own this point
        BOOST_REQUIRE_EQUAL(world.GetNode(pt).owner, 1);
        // Points at radius are border nodes, others player territory
        if(world.CalcDistance(pt, hqPos) == HQ_RADIUS)
            BOOST_REQUIRE(world.IsBorderNode(pt, 1));
        else
            BOOST_REQUIRE(world.IsPlayerTerritory(pt));
    }

 void LocalMapping::MapPointCulling()
 {
     // Check Recent Added MapPoints
     list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
     const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
     while(lit!=mlpRecentAddedMapPoints.end())
     {
         MapPoint* pMP = *lit;
         if(pMP->isBad())
         {
             lit = mlpRecentAddedMapPoints.erase(lit);
         }
         else if(pMP->GetFoundRatio()<0.25f )
         {
             pMP->SetBadFlag();
             lit = mlpRecentAddedMapPoints.erase(lit);
         }
         else if((nCurrentKFid-pMP->mnFirstKFid)>=2 && pMP->Observations()<=2)
         {
             
             pMP->SetBadFlag();
             lit = mlpRecentAddedMapPoints.erase(lit);
         }
         else if((nCurrentKFid-pMP->mnFirstKFid)>=3)
             lit = mlpRecentAddedMapPoints.erase(lit);
         else
             lit++;
     }
 }

void BundleRTS::updateNewPosesPoints() {
    //update the points
    for (MapPoint* mpt = pCoSLAM->curMapPts.getHead(); mpt; mpt = mpt->next) {
        if (mpt->lastFrame <= firstKeyFrame->f)
            continue;
//		if (mpt->bLocalDyn == TYPE_MAPPOINT_STATIC || mpt->bLocalDyn == TYPE_MAPPOINT_UNCERTAIN
//		)
        if (mpt->isLocalDynamic())
            updateStaticPointPosition(pCoSLAM->numCams, mpt,
                                      Const::PIXEL_ERR_VAR, false);
        else if (mpt->isLocalDynamic())
            updateDynamicPointPosition(pCoSLAM->numCams, mpt,
                                       Const::PIXEL_ERR_VAR, false);
    }
    for (MapPoint* mpt = pCoSLAM->actMapPts.getHead(); mpt; mpt = mpt->next) {
        if (mpt->lastFrame <= firstKeyFrame->f)
            continue;
//		if (mpt->bLocalDyn == TYPE_MAPPOINT_STATIC || mpt->bLocalDyn == TYPE_MAPPOINT_UNCERTAIN
//		)
        if (mpt->isLocalStatic())
            updateStaticPointPosition(pCoSLAM->numCams, mpt,
                                      Const::PIXEL_ERR_VAR, false);
        else if (mpt->isLocalDynamic())
            updateDynamicPointPosition(pCoSLAM->numCams, mpt,
                                       Const::PIXEL_ERR_VAR, false);
    }
}

bool StereoFrame::canView(const MapPoint& mapPoint) const
{
  boost::shared_lock<boost::shared_mutex> lock(stereo_frames_mutex_);

  const Eigen::Vector3d& point = mapPoint.GetPosition();

  // compute only once, since both cameras have the same orientation.
  bool similar_angle = false;
  {
    Eigen::Vector3d currentNormal = point - frameLeft_.GetPosition();
    currentNormal.normalize();

    // angle is in radians
    double angle = std::acos( ( mapPoint.GetNormal() ).dot( currentNormal ) );

    // Discard Points which were created from a greater 45 degrees pint of view.
    // TODO: pass this threshold as a parameter.
    similar_angle = angle < (M_PI / 4.0);
  }

  return similar_angle and ( frameLeft_.GetCamera().CanView( point ) or frameRight_.GetCamera().CanView( point ) );
}