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; }
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; }
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()); } }
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)); }
// 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(); }
// 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(); }
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; }
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; }
RectLandmark::RectLandmark(const Landmark& landmark) : Landmark(LandmarkType::RECT, landmark.getName(), landmark.isVisible()), position(landmark.getPosition2D()), size(landmark.getSize()) {}
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; } } }
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(); }
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"; }
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; }