Ejemplo n.º 1
0
bool RectLandmark::isEqual(const Landmark& landmark) const {
	if (landmark.getType() != LandmarkType::RECT)
		return false;
	float eps = 1e-6;
	return abs(getX() - landmark.getX()) < eps && abs(getY() - landmark.getY()) < eps
			&& abs(getWidth() - landmark.getWidth()) < eps && abs(getHeight() - landmark.getHeight()) < eps;
}
Ejemplo n.º 2
0
bool RectLandmark::isClose(const Landmark& landmark, const float similarity) const {
	if (landmark.getType() != LandmarkType::RECT)
		return false;
	Rect_<float> thisRect = getRect();
	Rect_<float> thatRect = landmark.getRect();
	float intersectionArea = (thisRect & thatRect).area();
	float unionArea = thisRect.area() + thatRect.area() - intersectionArea;
	return intersectionArea / unionArea >= similarity;
}
int main()
{
    std::vector<Landmark> landmarks;
    landmarks.reserve( 6 );

    Landmark buoy1( 11.0, 3.1, 0.0, "buoy", 0 );
    Landmark buoy2( 11.5, 3.0, 0.0, "buoy", 1 );
    Landmark buoy3( 10.5, 2.9, 0.0, "buoy", 2 );

    Landmark gate( 4.0, 2.5, 0.14, "gate" );

    Landmark pipe1( 6.0, 2.0, 0.2, "pipe", 0 );
    Landmark pipe2( 14.0, 3.0, 0.3, "pipe", 1 );

    // add all landmarks to "landmarks" in the order they were defined above
    landmarks.push_back( buoy1 );
    landmarks[1] = buoy2;
    *landmarks.begin() = buoy3;
    landmarks.back() = gate;
    landmarks.at( 3 ) = pipe1;
    landmarks.insert( landmarks.begin(), pipe2 );

    // for everything below this line, assume the landmarks have all been added to "landmarks" correctly

    // print all the landmarks
    {
        unsigned int i = 0;
        while( i <= landmarks.size() )
        {
            Landmark landmark = *( landmarks.begin() + i );
            std::cout << landmark.toString() << std::endl;
        }
    }

    // print all the landmarks
    {
        std::vector<Landmark>::iterator landmarks_it = landmarks.begin();
        while( landmarks_it != landmarks.end() )
        {
            std::cout << landmarks_it->toString() << std::endl;

            ++landmarks_it;
        }
    }

    // print all the landmarks
    {
        for( Landmark * landmark_ptr = &landmarks[0]; landmark_ptr != &landmarks[landmarks.size()]; ++landmark_ptr )
        {
            std::cout << landmark_ptr->toString() << std::endl;
        }
    }

    return 0;
}
Ejemplo n.º 4
0
void FeatureSelector::Input(const IImage& image, const ZDouble2Pair& pair, const Landmark& landmark, const Landmark& perfect) {
	assert(landmark.Count() == landmarkCount);
	assert(perfect.Count() == landmarkCount);
	for (size_t i = 0; i < samplers.size(); i++) {
		mat.Push(samplers[i].Query(image, pair, landmark));
	}

	for (size_t j = 0; j < landmarkCount; j++) {
		ZDouble2 d = landmark[j] - perfect[j];
		diff.Push(d.x());
		diff.Push(d.y());
	}
}
Ejemplo n.º 5
0
void Gui::GetDisplayImage(cv::Mat &image)
{
  cv::Mat out_left, out_right;
  std::unique_lock<std::mutex> lock = lock_gui();

  if(!lock)
    return;
  
  cv::drawKeypoints(current_frames_[0], current_keypoints_[0], out_left);
  cv::drawKeypoints(current_frames_[1], current_keypoints_[1], out_right);
  cv::hconcat(out_left, out_right, image);
  // Draw correspondences
  for(auto z : current_measurements_)
  {
    if(z.HasGoodMeasurement())
    {
      Eigen::MatrixXd p1 = z.Pixel(0);
      Eigen::MatrixXd p2 = z.Pixel(1);

      line(image, cv::Point(p1(0),p1(1)), cv::Point(p2(0)+current_frames_[0].size().width,p2(1)),cv::Scalar(0,255,0.,1.0));
    }
  }

  /* Draw landmark tracks */
  for(size_t row=0; row < track_info_.size(); ++row) {
    for(MultiViewMeasurement& z: track_info_[row]->measurements_){
        Landmark landmark;
        if(!map_->GetLandmark(z.id().landmark_id, &landmark)){
          ROS_INFO("Can't find landmark");
          continue;
        }
        const std::vector<MeasurementId>& msr_ids = landmark.GetFeatureTrackRef();

        MultiViewMeasurement z1,z2;
        int cam_id = 0;
        for (unsigned int ii=0; ii < msr_ids.size()-1; ++ii) {
          map_->GetMeasurement(msr_ids[ii], z1);
          map_->GetMeasurement(msr_ids[ii+1], z2);
          if (z1.HasGoodMeasurementInCam(cam_id) && z2.HasGoodMeasurementInCam(cam_id)){
            const Eigen::Vector2t& p1 = z1.Pixel(cam_id);
            const Eigen::Vector2t& p2 = z2.Pixel(cam_id);
            line(image, cv::Point(p1(0),p1(1)), cv::Point(p2(0),p2(1)),cv::Scalar(255,0.,0.,1.0));
          }
        }
    }

  }
}
double ManualImage2ImageRegistrationWidget::getAccuracy(QString uid)
{
    DataPtr fixedData = mServices->registration()->getFixedData();
    if (!fixedData)
        return 1000.0;
    DataPtr movingData = mServices->registration()->getMovingData();
    if (!movingData)
        return 1000.0;

    Landmark masterLandmark = fixedData->getLandmarks()->getLandmarks()[uid];
    Landmark targetLandmark = movingData->getLandmarks()->getLandmarks()[uid];
    if (masterLandmark.getUid().isEmpty() || targetLandmark.getUid().isEmpty())
        return 1000.0;

    Vector3D p_master_master = masterLandmark.getCoord();
    Vector3D p_target_target = targetLandmark.getCoord();
    Transform3D rMmaster = fixedData->get_rMd();
    Transform3D rMtarget = movingData->get_rMd();

    Vector3D p_target_r = rMtarget.coord(p_target_target);
    Vector3D p_master_r = rMmaster.coord(p_master_master);

    double  targetPoint[3];
    double  masterPoint[3];
    targetPoint[0] = p_target_r[0];
    targetPoint[1] = p_target_r[1];
    targetPoint[2] = p_target_r[2];
    masterPoint[0] = p_master_r[0];
    masterPoint[1] = p_master_r[1];
    masterPoint[2] = p_master_r[2];

    return (vtkMath::Distance2BetweenPoints(targetPoint, masterPoint));
}
Ejemplo n.º 7
0
// delete unconfirmed landmarks from the given landmarks vector. buffer is used as a buffer and its contents are cleared.
void deleteUnconfirmedLandmarks(std::vector<Landmark *> * toBeCleaned, std::vector<Landmark *> * buffer, std::list<Landmark *> * total_list){
  
  buffer->clear();
  for(unsigned int i=0; i<toBeCleaned->size(); i++){
    Landmark * lm = (*toBeCleaned)[i];
    
    if(lm->isConfirmed()){
      buffer->push_back(lm);
    }
    else{
      total_list -> remove(lm);
      delete lm;
    }
  }
  
  toBeCleaned->clear();
  
  for(unsigned int i=0; i<buffer->size(); i++){
    toBeCleaned->push_back((*buffer)[i]);
  }
  buffer->clear();
}
Ejemplo n.º 8
0
// delete unconfirmed landmarks from the given landmarks list. buffer is used as a buffer and its contents are cleared.
void deleteUnconfirmedLandmarks(std::list<Landmark *> * landmarks, std::vector<Landmark *> * buffer){
  
  buffer->clear();
  std::list<Landmark *>::iterator iter;
  for(iter = landmarks->begin(); iter!=landmarks->end(); iter++){
    Landmark * lm = *iter;
    
    if(lm->isConfirmed()){
      buffer->push_back(lm);
    }
    else{
      delete lm;
    }
  }
  
  landmarks->clear();
  
  for(unsigned int i=0; i<buffer->size(); i++){
    landmarks->push_back((*buffer)[i]);
  }
  buffer->clear();
}
double LandmarkRegistrationWidget::getAccuracy(QString uid)
{
	DataPtr fixedData = mServices->registration()->getFixedData();
	if (!fixedData)
		return 1000.0;

	Landmark masterLandmark = fixedData->getLandmarks()->getLandmarks()[uid];
	Landmark targetLandmark = this->getTargetLandmarks()[uid];
	if (masterLandmark.getUid().isEmpty() || targetLandmark.getUid().isEmpty())
		return 1000.0;

	Vector3D p_master_master = masterLandmark.getCoord();
	Vector3D p_target_target = targetLandmark.getCoord();
	Transform3D rMmaster = fixedData->get_rMd();
	Transform3D rMtarget = this->getTargetTransform();

	Vector3D p_target_r = rMtarget.coord(p_target_target);
	Vector3D p_master_r = rMmaster.coord(p_master_master);

	return (p_target_r - p_master_r).length();
}
Ejemplo n.º 10
0
    cv::Mat CroppedImage(cv::Mat img,Landmark<cv::Point> eye,Landmark<cv::Point> lips, Landmark<cv::Point>nose)
    {
        cv::Point Eye=eye.GetLocation();
        cv::Point Lips=lips.GetLocation();
        cv::Point Nose=nose.GetLocation();
        int minx, maxx;

        if(Eye.x<Nose.x) //right side
        {
            int minX=Eye.x;
            if (minX>Lips.x)
            {
                minX=Lips.x;
            }
            int maxX=Nose.x;
            minx=4*minX-3*maxX; //horizontal distance nose-eye = quarter of the face
            if(minx<0)
            {
                minx=0;
            }
            maxx=maxX+20;//nose supposed to be on the contour
            if(maxx>img.cols)
            {
                maxx=img.cols;
            }
        }
        else //left side
        {
            int maxX=Eye.x;
            if (maxX<Lips.x)
            {
                maxX=Lips.x;
            }
            int minX=Nose.x;

            minx=minX-20;
            if(minx<0)
            {
                minx=0;
            }
            maxx=4*maxX-3*-minX;//nose supposed to be on the contour
            if(maxx>img.cols)
            {
                maxx=img.cols;
            }
        }

        int minY=Eye.y;
        int maxY=Nose.y;
        int miny=4*minY-3*maxY; //vertical distance nose-eye = 1/7 of the face
        if(miny<0)
        {
            miny=0;
        }
        int maxy=4*maxY-3*minY;
        if(maxy>img.rows)
        {
            maxy=img.rows;
        }

        cv::Rect roi(minx,miny,maxx-minx,maxy-miny);
        cv::Mat crop=img(roi);

        return crop;

    }
Ejemplo n.º 11
0
    std::vector<cv::Mat> Backgroung(cv::Mat img,Landmark<cv::Point> eye,Landmark<cv::Point> lips, Landmark<cv::Point>nose)
    {
        cv::Point Eye=eye.GetLocation();
        cv::Point Lips=lips.GetLocation();
        cv::Point Nose=nose.GetLocation();
        int minx, maxx;

        if(Eye.x<Nose.x) //right side
        {
            int minX=Eye.x;
            if (minX>Lips.x)
            {
                minX=Lips.x;
            }
            int maxX=Nose.x;
            minx=4*minX-3*maxX; //horizontal distance nose-eye = quarter of the face
            if(minx<0)
            {
                minx=0;
            }
            maxx=maxX+20;//nose supposed to be on the contour
            if(maxx>img.cols)
            {
                maxx=img.cols;
            }std::cout<<"he"<<std::endl;
        }
        else //left side
        {
            int maxX=Eye.x;
            if (maxX<Lips.x)
            {
                maxX=Lips.x;
            }
            int minX=Nose.x;

            minx=minX-20;
            if(minx<0)
            {
                minx=0;
            }
            maxx=/*maxX+3*(maxX-minX)*/4*maxX-3*minX;//nose supposed to be on the contour
            if(maxx>img.cols)
            {
                maxx=img.cols;
            }
        }

        int minY=Eye.y;
        int maxY=Nose.y;
        int miny=4*minY-3*maxY; //vertical distance nose-eye = 1/7 of the face
        if(miny<0)
        {
            miny=0;
        }
        int maxy=4*maxY-3*minY;
        if(maxy>img.rows)
        {
            maxy=img.rows;
        }

        std::vector<cv::Mat> bg;
        if (minx!=0 && miny!=0)
        {
            bg.push_back(img(cv::Rect(0,0,minx,miny)));
        }
        if (minx!=0 )
        {
            bg.push_back(img(cv::Rect(0,miny,minx,maxy-miny)));
        }
        if (miny!=0 )
        {
            bg.push_back(img(cv::Rect(minx,0,maxx-minx,miny)));
        }
        if (minx!=0 && maxy!=img.rows)
        {
            bg.push_back(img(cv::Rect(0,maxy,minx,img.rows-maxy)));
        }
        if (miny!=0 && maxx!=img.cols)
        {
            bg.push_back(img(cv::Rect(maxx,0,img.cols-maxx,miny)));
        }
        if (maxy!=img.rows)
        {
            bg.push_back(img(cv::Rect(minx,maxy,maxx-minx,img.rows-maxy)));
        }
        if(maxx!=img.cols)
        {
            bg.push_back(img(cv::Rect(maxx,miny,img.cols-maxx,maxy-miny)));
        }
        if (maxx!=img.cols && maxy!=img.rows)
        {
            bg.push_back(img(cv::Rect(maxx,maxy,img.cols-maxx,img.rows-maxy)));
        }

        return bg;
    }
Ejemplo n.º 12
0
RectLandmark::RectLandmark(const Landmark& landmark) :
		Landmark(LandmarkType::RECT, landmark.getName(), landmark.isVisible()), position(landmark.getPosition2D()), size(landmark.getSize()) {}
Ejemplo n.º 13
0
void generateGraphFile(char* filename, std::vector<RobotPosition *> * poses, std::vector<RobotPosition*>* transformations, std::list<Landmark *> * landmarks){
  std::string graphname = basename(filename);
  graphname = graphname.substr(0,graphname.length()-4);
  std::ofstream g2oout((graphname+(std::string("_graph.g2o"))).c_str());
  
  // poses
  for(unsigned int i=0; i<poses->size(); i++){
    RobotPosition * pose = (*poses)[i];
    RobotPosition * transf = (*transformations)[i];
    RobotPosition * prev_pose;
    // unsigned int prev_id;
    
    g2oout << "VERTEX_SE2 " << pose->id() << " " << pose->x() << " " << pose->y() << " " << pose->theta() << std::endl;
    
    if(i>0){
      prev_pose = (*poses)[i-1];
      g2oout << "EDGE_SE2 " << prev_pose->id() << " " << pose->id() << " " << transf->x() << " " << transf->y() << " " << transf->theta() << " 500 0 0 500 0 100" << std::endl;
    }
  }
  
  // landmarks
  std::list<Landmark *>::iterator iter;
  for(iter=landmarks->begin(); iter!=landmarks->end(); iter++){
    Landmark * lmark = *iter;
    
    if(!lmark->isConfirmed()){
      continue;
    }
    
    if(!lmark->hasId()){
      lmark->setId(next_id++);
      lmark->idAssigned();
    }
    // add the landmark
    g2oout << "VERTEX_XY " << lmark->id() << " " << lmark->x() << " " << lmark->y() << std::endl;
    
    // add the associated bearing constraints
    for(unsigned int o=0; o<lmark->getObservations()->size(); o++){
      Observation * observation= (*(lmark->getObservations()))[o];
      RobotPosition * pose = observation->pose;
      
      g2oout << "EDGE_BEARING_SE2_XY " << pose->id() << " " << lmark->id() << " " << observation->bearing << " 200" << std::endl;
    }
  }
}
Ejemplo n.º 14
0
void populateGraph(std::vector<RobotPosition *> * poses, std::vector<RobotPosition*> * transformations, std::list<Landmark *> * landmarks){
  std::cout << "clearing optimizer..." << std::endl;
  // optimizer->clear();
  
  // put poses in the graph
  for(unsigned int i=0; i<poses->size(); i++){
    if((*poses)[i]->already_in_graph){
      continue;
    }
    
    // add this pose to the graph
    optimizer->addVertex((*poses)[i]);
    ((*poses)[i])->already_in_graph = true;
    
    if(i>0){
      // add the constraint regarding the rototranslation performed from the previous step
      g2o::EdgeSE2 * odom_edge = new g2o::EdgeSE2;
      odom_edge->vertices()[0] = (*poses)[i-1];
      odom_edge->vertices()[1] = (*poses)[i];
      g2o::SE2 * measurement = new g2o::SE2((*transformations)[i]->x(), (*transformations)[i]->y(), (*transformations)[i]->theta());
      odom_edge->setMeasurement(*measurement);
      odom_edge->setInformation(*odom_info);
      
      optimizer->addEdge(odom_edge);
    }
  }
  
  // put landmarks in the graph
  std::list<Landmark *>::iterator iter;
  for(iter = landmarks->begin(); iter!=landmarks->end(); iter++){
    
    Landmark * lm = *iter;
    
    if(!lm->isConfirmed()){
      continue;
    }
    
    if(lm->already_in_graph){
      continue;
    }
    
    lm->already_in_graph = true;
    
    if(!lm->hasId()){
      lm->setId(next_id++);
      lm->idAssigned();
    }
    
    optimizer->addVertex(lm);
    
    for(unsigned int o=0; o<lm->getObservations()->size(); o++){
      Observation * observation= (*(lm->getObservations()))[o];
      RobotPosition * pose = observation->pose;
      
      g2o::EdgeSE2PointXYBearing* obs_edge =  new g2o::EdgeSE2PointXYBearing;
      obs_edge->vertices()[0] = pose;
      obs_edge->vertices()[1] = lm;
      obs_edge->setMeasurementData(&(observation->bearing));
      obs_edge->setInformation(*obs_info);
      robust_kernel = new g2o::RobustKernelCauchy();
      obs_edge->setRobustKernel(robust_kernel);
      
      optimizer->addEdge(obs_edge);
    }
  }
  
  optimizer->vertex(0)->setFixed(true);
  optimizer->initializeOptimization();
}
Ejemplo n.º 15
0
void tryToUnderstand1(RobotPosition * pose,  std::list<Landmark *>* landmarks,std::vector<Landmark *> *  last_observations, std::vector<Landmark *> *  propagate_observations){
  double rpose[3];
  pose->getEstimateData(rpose);
  
  propagate_observations->clear();
  
  std::cout << "Robot Position:\t" << rpose[0] << "\t" << rpose[1] << "\t" << rpose[2] << "\n";
  
  double expected[last_observations->size()];	//	this will contain the projections of the previously seen landmarks in the current pose
  // I.E. this tells where we expect to see the landmarks propagated from the last step

  bool use_general_angle_tolerance[last_observations->size()];	// this will tell if the angle tolerance to be used is the general one or the bearing-only based one

  int associations[pose->observations.size()];	// this will contain the candidate associations, that will become effective ONLY IF there will be no ambiguity
  for(unsigned int i=0; i<pose->observations.size(); i++){
    associations[i] = -1;
  }

  std::cout << "We expect to see stuff at:\n";
  for(unsigned int i = 0; i<last_observations->size(); i++){	// populate the expected array
    Landmark * lm = (*last_observations)[i];

    if(lm->getObservations()->size() > 1){	// first case: landmark already has an estimated position
      expected[i] = computeAngle(lm, pose);
      use_general_angle_tolerance[i] = true;
    }
    else{	// second case: landmark has been seen only once up to now, hence there is no position estimation
      double prev_seen = (*(lm->getObservations()))[0]->bearing;
      double prev_theta = (*(lm->getObservations()))[0]->pose->theta();
      double rotated = computeAnglesDifference(pose->theta(), prev_theta);
      expected[i] = prev_seen-rotated;
      use_general_angle_tolerance[i] = false;
    }

    std::cout << expected[i] << "\n";
  }

  std::cout << "observations:\n";
  for(unsigned int obs_counter=0; obs_counter < pose->observations.size(); obs_counter++){	// for every observation in the current position
    std::cout << (pose->observations)[obs_counter].bearing << ": ";
    // Try to assign it to a previously generated landmark
    if(last_observations->size() > 0){

      unsigned int closer_index;
      unsigned int second_closer_index;
      // compute closer_index and second_closer_index.

      double distances[last_observations->size()];
      for(unsigned int i=0; i < last_observations->size(); i++){
	distances[i] = d_abs(computeAnglesDifference(expected[i], pose->observations[obs_counter].bearing));
      }	//	now 'distances' contains the distance between the currently considered observation and the expected values

      closer_index = 0;
      if(last_observations->size() == 1){
	second_closer_index = 0;
      }
      else{
	second_closer_index = 1;
	if(distances[1] < distances[0]){
	  closer_index = 1;
	  second_closer_index = 0;
	}
      }
      for (unsigned int i = 1; i < last_observations->size(); i++){
	if (distances[i] <  distances[closer_index]){
	  second_closer_index = closer_index;
	  closer_index = i;
	}
	else{
	  if(distances[i] <  distances[second_closer_index]){
	    second_closer_index = i;
	  }
	}
      }
      // if closer is "close enough" and second_closer is "far enough" (I.E. there is no ambiguity)
      // associate the current observation to 'closer'.
      double tolerance = GENERAL_ANGLE_TOLERANCE;
      if(!use_general_angle_tolerance[closer_index]){
	tolerance = BEAR_ONLY_ANGLE_TOLERANCE;
      }
      std::cout << "tolerance is " << tolerance << "\n";
      if(distances[closer_index] < tolerance){
	if((second_closer_index == closer_index) || (distances[second_closer_index] > SECOND_ANGLE_MIN_DISTANCE)){
	  associations[obs_counter] = closer_index;
	  std::cout << "close to " << expected[closer_index] << "\n";
	}
	else{
	  std::cout << "close to " << expected[closer_index] << " but also to " << expected[second_closer_index] << "\n";
	}
      }
      else{
	std:: cout << "is far from everything\n";
      }

      // NOTE:	Do not add the observation to the set inside the landmark corresponding to
      //			'closer' yet, because, if two or more NEW observations are associated to the same
      //			landmark, none of them will be added.
    }
  }

  // add the computed observations to the landmarks, but only if there is no ambiguity

  bool associated[last_observations->size()];	//	associated[i] will be true if there is at least 1 new observation associated to the i-th landmark in last_observations
  bool ambiguous[last_observations->size()];	//	ambiguous[i] will be true if there are at least 2 new observations associated to the i-th landmark in last_observations
  // initialize the arrays just created
  for(unsigned int i=0; i<last_observations->size(); i++){
    associated[i] = false;
    ambiguous[i] = false;
  }

  // tell which ones are ambiguous
  for(unsigned int i=0; i < pose->observations.size(); i++){
    unsigned int value = associations[i];
    if(value!=-1){
      if (!associated[value]){
	associated[value] = true;
      }
      else{	//	associated is true, hence there is already a new observation (or more) polling for the landmark
	ambiguous[value] = true;
      }
    }
  }

  // add non-ambiguous observations to the corresponding landmarks
  // and create new landmarks for the new observed unassociated observations
  std::cout << "adding observations to landmarks\n";
  for(unsigned int i=0; i < pose->observations.size(); i++){
    std::cout << "obs " << i << " ";
    if(associations[i] != -1){
      if(!ambiguous[associations[i]]){
	std::cout << "tailed to " << associations[i] << "\n";
	Landmark * lm = (*last_observations)[associations[i]];
	lm->addObservation(&(pose->observations[i]));
	lm->estimatePosition();
	lm->checkConfirmed(_confirm_obs);
	double newpos[2];
	(*last_observations)[associations[i]]->getPosition(newpos);
	std::cout << "\tnew position estimated:\t" << newpos[0] << "\t" << newpos[1] << "\n";
	propagate_observations->push_back((*last_observations)[associations[i]]);
      }
      else{
	std::cout << "should be tailed to " << associations[i] << ", but there is ambiguity\n";
      }
    }
    else{	// unassociated
      std::cout << "is a new landmark\n";
      Landmark * lm = new Landmark();
      lm->addObservation (&(pose->observations[i]));
      //			lm->estimatePosition();
      landmarks->push_back(lm);
      propagate_observations->push_back(lm);
    }
  }

  std::cout << "now checking landmarks from previous step\n";
  for(unsigned int i = 0; i<last_observations->size(); i++){
    std::cout << "prev_obs " << i << " ";
    Landmark * lm = (*last_observations)[i];
    if(!associated[i]){
      // for the landmarks expected to be seen that are not in the current set of observations, we have two cases:
      if(lm->isConfirmed()){
	// case one: the landmark has been confirmed observations, then it is reliable
	std::cout << "was old enough, CONFIRMED\n";
      }
      else{
	// case two: the landmark is pretty unreliable, then it is trashed
	std::cout << "was too young, REMOVED\n";
	landmarks->remove(lm);
	delete lm;
      }
    }
    else{
      if(ambiguous[i]){
	std::cout << "was ambiguous,";
	if(lm->isConfirmed()){
	  std::cout << " but it was considered reliable, PROPAGATED" << std::endl;
	  propagate_observations->push_back(lm);
	}
	else{
	  std::cout << " and it was pretty young, REMOVED" << std::endl;
	  landmarks->remove(lm);
	  delete lm;
	}
      }
      else{
	std::cout << "has been REINFORCED\n";
      }
    }
  }
  std::cout << "\n\n";
}
Ejemplo n.º 16
0
void
MapForm::OnReverseGeocodingRequestResultReceivedN(RequestId requestId, const IGeocodingServiceProvider& provider,
	Osp::Base::Collection::IList* pLandmarks, result r, const Osp::Base::String& errorCode,
	const Osp::Base::String& errorMsg)
{
	if (E_SUCCESS == r && pLandmarks && pLandmarks->GetCount() > 0)
	{
		Landmark* pLandmark = static_cast<Landmark*>(pLandmarks->GetAt(0));
		const AddressInfo* pAddressInfo = pLandmark->GetAddressInfo();
		const QualifiedCoordinates* pCoords = pLandmark->GetQualifiedCoordinates();
		if (pAddressInfo && pCoords)
		{
			String streetNumber = pAddressInfo->GetField(ADDRESS_FIELD_STREET_NUMBER);
			String streetName = pAddressInfo->GetField(ADDRESS_FIELD_STREET_NAME);
			String city = pAddressInfo->GetField(ADDRESS_FIELD_CITY);
			String state = pAddressInfo->GetField(ADDRESS_FIELD_STATE);
			String postalCode = pAddressInfo->GetField(ADDRESS_FIELD_POSTAL_CODE);
			String address = "Latitude: " ;
			address += Double::ToString(pCoords->GetLatitude());
			address += L"\n";
			address += L"Longitude: ";
			address += Double::ToString(pCoords->GetLongitude());
			address += L"\n";
			address += L"\n";

			if(!streetNumber.IsEmpty())
			{
				address += 	streetNumber;
			}

			if (!streetName.IsEmpty())
			{
				address += L" ";
				address += streetName;
			}
			if (!city.IsEmpty())
			{
				address += L" ";
				address += city;
			}
			if (!state.IsEmpty())
			{
				address += L", ";
				address += state;
			}
			if (!postalCode.IsEmpty())
			{
				address += L" ";
				address += postalCode;
			}

			int modalResult;
			MessageBox mbox;
			String title(L"Address Information");
			mbox.Construct(title, address, MSGBOX_STYLE_OK, 0);
			mbox.ShowAndWait(modalResult);
		}
	}
	else if (E_SERVER == r)
	{
		int modalResult;
		MessageBox mbox;
		String title(L"It failed to get the address.");
		mbox.Construct(title, errorMsg, MSGBOX_STYLE_OK, 0);
		mbox.ShowAndWait(modalResult);
	}
	if (pLandmarks) pLandmarks->RemoveAll(true);
	delete pLandmarks;
}