//================================================================
WirePlane::Measurement WirePlane::measurement(double wireNumber) const {
ROOT::Math::XYZPoint wireCenter = center_ +
mvector_ * wireSpacing_ * (wireNumber - centralWire_);
return Measurement(dir_,
(dir_ == U) ? wireCenter.x() : wireCenter.y()
);
}
void ManualController::updateParticleLocalizer(double dt) {
Robot::Movement movement = robot->getMovement();
Measurements measurements;
Object* yellowGoal = vision->getLargestGoal(Side::YELLOW);
Object* blueGoal = vision->getLargestGoal(Side::BLUE);
if (yellowGoal != NULL) {
measurements["yellow-center"] = Measurement(Math::min(Math::max(yellowGoal->distance, 0.0f), Config::fieldWidth), yellowGoal->angle);
}
if (blueGoal != NULL) {
measurements["blue-center"] = Measurement(Math::min(Math::max(blueGoal->distance, 0.0f), Config::fieldWidth), blueGoal->angle);
}
//float velocityMagnitude = Math::Vector(movement.velocityX, movement.velocityY).getLength();
//if (velocityMagnitude > 0.05 && measurements.size() > 0) {
particleLocalizer.update(measurements);
particleLocalizer.move(movement.velocityX, movement.velocityY, movement.omega, dt, measurements.size() == 0 ? true : false);
//}
Math::Position position = particleLocalizer.getPosition();
}
Measurement DataClass::measurement(String name,
float (*measureValue)(), Timer time) {
return Measurement(name, measureValue, time);
}
Measurement DataClass::measurement(String name,
float (*measureValue)()) {
return Measurement(name, measureValue, _timer);
}
void StereoFrame::FindMatches(const Measurement::Source source,
const std::aligned_vector<Eigen::Vector3d>& points, const std::vector<cv::Mat>& descriptors,
const cv::DescriptorMatcher& descriptorMatcher,
const size_t matchingNeighborhoodThreshold, const double matchingDistanceThreshold,
std::list<size_t>& matchedIndexes, std::list<Measurement>& measurements
) const
{
boost::unique_lock<boost::shared_mutex> lock(stereo_frames_mutex_);
// TODO: maybe is useful to pass right descriptors too
std::list<std::pair<size_t, size_t> > measurementsLeft, measurementsRight;
#if 1
/* parallel find match */
#if SHOW_PROFILING && PROFILE_INTERNAL_MATCH
sptam::Timer t_left, t_right;
#endif
#if SHOW_PROFILING && PROFILE_INTERNAL_MATCH
t_left.start();
#endif
std::thread thread_left([&]() { measurementsLeft = frameLeft_.FindMatches(points, descriptors, descriptorMatcher, matchingDistanceThreshold, matchingNeighborhoodThreshold);
#if SHOW_PROFILING && PROFILE_INTERNAL_MATCH
t_left.stop();
#endif
});
#if SHOW_PROFILING && PROFILE_INTERNAL_MATCH
t_right.start();
#endif
std::thread thread_right([&]() { measurementsRight = frameRight_.FindMatches(points, descriptors, descriptorMatcher, matchingDistanceThreshold, matchingNeighborhoodThreshold);
#if SHOW_PROFILING && PROFILE_INTERNAL_MATCH
t_right.stop();
#endif
});
thread_left.join();
thread_right.join();
#if SHOW_PROFILING && PROFILE_INTERNAL_MATCH
WriteToLog(" xx findMatches-left: ", t_left);
WriteToLog(" xx findMatches-right: ", t_right);
#endif
#else
/* sequential find match */
measurementsLeft = frameLeft_.FindMatches(points, descriptors, descriptorMatcher, matchingDistanceThreshold, matchingNeighborhoodThreshold);
measurementsRight = frameRight_.FindMatches(points, descriptors, descriptorMatcher, matchingDistanceThreshold, matchingNeighborhoodThreshold);
#endif
#if SHOW_PROFILING
sptam::Timer t_process;
t_process.start();
#endif
// This is messy but efficient
auto it_left = measurementsLeft.begin();
auto it_right = measurementsRight.begin();
// iterate while there is something to be added to matches.
while ( it_left != measurementsLeft.end() or it_right != measurementsRight.end() )
{
// if both have the same source mapPoint, add a stereo measurement.
if ( it_left != measurementsLeft.end() and it_right != measurementsRight.end() and it_left->first == it_right->first )
{
// Create the Measurement
measurements.push_back( Measurement(source,
frameLeft_.GetFeatures().GetKeypoint( it_left->second ), frameLeft_.GetFeatures().GetDescriptor( it_left->second ),
frameRight_.GetFeatures().GetKeypoint( it_right->second ), frameRight_.GetFeatures().GetDescriptor( it_right->second )
));
frameLeft_.SetMatchedKeyPoint( it_left->second );
frameRight_.SetMatchedKeyPoint( it_right->second );
matchedIndexes.push_back( it_left->first );
++ it_left;
++ it_right;
}
// if the next index to process is the left one.
else if ( it_right == measurementsRight.end() or ( it_left != measurementsLeft.end() and it_left->first < it_right->first ) )
{
// Create the Measurement
measurements.push_back( Measurement(Measurement::LEFT, source, frameLeft_.GetFeatures().GetKeypoint( it_left->second ), frameLeft_.GetFeatures().GetDescriptor( it_left->second )) );
frameLeft_.SetMatchedKeyPoint( it_left->second );
matchedIndexes.push_back( it_left->first );
++ it_left;
}
// if the next index to process is the right one.
else
{
measurements.push_back( Measurement(Measurement::RIGHT, source, frameRight_.GetFeatures().GetKeypoint( it_right->second ), frameRight_.GetFeatures().GetDescriptor( it_right->second )) );
frameRight_.SetMatchedKeyPoint( it_right->second );
matchedIndexes.push_back( it_right->first );
++ it_right;
}
}
#if SHOW_PROFILING
t_process.stop();
WriteToLog(" XX findMatches-process: ", t_process);
#endif
}
void StereoFrame::TriangulatePoints(const RowMatcher& matcher, std::aligned_vector<MapPoint> &points, std::vector<Measurement> &measurements)/* const*/
{
#if defined(SHOW_PROFILING) && PROFILE_INTERNAL_TRIANGULATE
sptam::Timer t_lock, t_create;
t_lock.start();
#endif
boost::unique_lock<boost::shared_mutex> lock(stereo_frames_mutex_);
#if defined(SHOW_PROFILING) && PROFILE_INTERNAL_TRIANGULATE
t_lock.stop();
#endif
// TODO ImageFeatures podria guardar internamente el vector
// de los unmatched, y descartar los matched, total nunca más los uso.
// Esto permitiría recibir acá una referencia
// retrieve unmatched features from both frames.
std::vector<cv::KeyPoint> unmatchedKeypointsLeft, unmatchedKeypointsRight;
cv::Mat unmatchedDescriptorsLeft, unmatchedDescriptorsRight;
std::vector<size_t> indexesLeft, indexesRight;
// Get descritptors for both images
frameLeft_.GetUnmatchedKeyPoints(unmatchedKeypointsLeft, unmatchedDescriptorsLeft, indexesLeft);
frameRight_.GetUnmatchedKeyPoints(unmatchedKeypointsRight, unmatchedDescriptorsRight, indexesRight);
std::list<std::pair<size_t, size_t>> matches;
#if defined(SHOW_PROFILING) && PROFILE_INTERNAL_TRIANGULATE
t_create.start();
#endif
CreatePoints(
matcher,
unmatchedKeypointsLeft, unmatchedDescriptorsLeft,
unmatchedKeypointsRight, unmatchedDescriptorsRight,
points, matches
);
#if defined(SHOW_PROFILING) && PROFILE_INTERNAL_TRIANGULATE
t_create.stop();
#endif
measurements.reserve( matches.size() );
for ( auto pair : matches )
{
size_t idxLeft = pair.first;
size_t idxRight = pair.second;
frameLeft_.SetMatchedKeyPoint( indexesLeft[ idxLeft ] );
frameRight_.SetMatchedKeyPoint( indexesRight[ idxRight] );
measurements.push_back( Measurement(
Measurement::SRC_TRIANGULATION,
unmatchedKeypointsLeft[ idxLeft ], unmatchedDescriptorsLeft.row( idxLeft ),
unmatchedKeypointsRight[ idxRight ], unmatchedDescriptorsRight.row( idxRight )
) );
}
#if defined(SHOW_PROFILING) && PROFILE_INTERNAL_TRIANGULATE
WriteToLog(" xx triangulate_points-lockStereo: ", t_lock);
WriteToLog(" xx triangulate_points-create: ", t_create);
#endif
}