Skip to content

Commit

Permalink
feat(PositionProcessing): Detect new pattern (#37)
Browse files Browse the repository at this point in the history 
* Added id to the blobs.

* Starting generic implementation to make it compatible with both patterns.

* First version of tracking new pattern. With GUI following the pattern.

* Add static video

* wip: changing new-pattern detection (failed)

* fix: position detection

* feat: Using Euclidean distance to calculate the distance between blobs

* wip: decreasing the maximum distance between blobs

* wip: adjusting "blobMaxDist"

* fix: new detection to pattern

* fix: adjust tag colors render

* feat: Sort regions by leftmost blob

* refactor: update distance function

* Refactor: changes filterPattern

* refactor: update distance function

* Refactor: changes filterPattern

* First version of tracking new pattern. With GUI following the pattern.

* feat(PositionProcessing): Change pattern detection and robot id calculate (#36)

* fix(PositionProcessing): removes old pattern detection

* wip: prints

* feat(videos): add new static video

* feat(PositionProcessing): add new robot id detection

* fix(RobotWidget): remove "gambiarra"

* fix(server): remove "gambiarra" id

* feat(PositionProcessing): Add old pattern detection possibility

* Revert "Merge branch 'new-pattern' into first-version"

This reverts commit 040de64.

* feat(Videos): add new video

* fix(RobotWidget): fix robot color at interface

* feat(PositionProcessing): add new filter for pattern detection

* fix(RobotWidget): adjusting color ids order

* fix(PositionProcessing): regions with less than three blobs skipped

* fix(Receiver): Multicast configuration

* fix(PositionProcessing): adjusting robot id (pink,green)

* fix(PositionProcessing): Checking valid id

* chore(videos): adding new tag pattern videos

* refactor(RobotWidget): update pattern draw

* refactor(PositionProcessing): update findTeam

* [wip] fix filterPattern

* [wip] fix id

* feat(PositionProcessing): add kalman filter

* fix(RobotWiget): adjust pattern draw

---------

Co-authored-by: acrucha <[email protected]>

---------

Co-authored-by: acrucha <[email protected]>
  • Loading branch information
@ersaraujo @acrucha
ersaraujo and acrucha authored Sep 20, 2023
1 parent 2157385 commit 8d41dcc
Show file tree
Hide file tree
Showing 14 changed files with 293 additions and 85 deletions.
10 changes: 4 additions & 6 deletions client/python/receiver.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,13 +22,11 @@ def __init__(self,
self.vision_ip = vision_ip
self.vision_port = vision_port

self.vision_sock = socket.socket(
socket.AF_INET, socket.SOCK_DGRAM)
self.vision_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.vision_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.vision_sock.setsockopt(socket.IPPROTO_IP,
socket.IP_MULTICAST_TTL, 128)
self.vision_sock.setsockopt(socket.IPPROTO_IP,
socket.IP_MULTICAST_LOOP, 1)
self.vision_sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 128)
self.vision_sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1)
self.vision_sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, struct.pack("=4sl", socket.inet_aton(self.vision_ip), socket.INADDR_ANY))
self.vision_sock.bind((self.vision_ip, self.vision_port))

# self.vision_sock.setblocking(True)
Expand Down
4 changes: 2 additions & 2 deletions src/Network/visionServer/server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ void VisionServer::send(std::vector<Entity> &entities) {
} else{
if((int)entities[i].team() == Color::YELLOW){
SSL_DetectionRobot *robot = frame->mutable_robots_yellow()->Add();
robot->set_robot_id(entities[i].id()-200);
robot->set_robot_id(entities[i].id());
robot->set_x(entities[i].position().x * 10);
robot->set_y(entities[i].position().y * 10);
robot->set_orientation(entities[i].angle());
Expand All @@ -52,7 +52,7 @@ void VisionServer::send(std::vector<Entity> &entities) {
robot->set_pixel_y(robot->y()*100);
}else if((int)entities[i].team() == Color::BLUE){
SSL_DetectionRobot *robot = frame->mutable_robots_blue()->Add();
robot->set_robot_id(entities[i].id()-100);
robot->set_robot_id(entities[i].id());
robot->set_x(entities[i].position().x * 10);
robot->set_y(entities[i].position().y * 10);
robot->set_orientation(entities[i].angle());
Expand Down
1 change: 1 addition & 0 deletions src/Vision/PositionProcessing/BlobDetection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,6 +108,7 @@ void BlobDetection::findBlobs(cv::Mat& debugFrame) {
blob[cor][_clusterCount[cor]].position.y = r.sumX / this->_runs[i][j].areaBlob;
blob[cor][_clusterCount[cor]].area = r.areaBlob;
blob[cor][_clusterCount[cor]].valid = true;
blob[cor][_clusterCount[cor]].id = _clusterCount[cor];
blob[cor][_clusterCount[cor] + 1].valid = false;
_clusterCount[cor]++;

Expand Down
261 changes: 196 additions & 65 deletions src/Vision/PositionProcessing/PositionProcessing.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#include "PositionProcessing.h"

#include "Utils/EnumsAndConstants.h"

void PositionProcessing::saveXML()
{
Expand All @@ -26,14 +26,13 @@ void PositionProcessing::matchBlobs(cv::Mat& debugFrame){
static Players empty_players;

vss.setPlayers(empty_players);
// printf("Team Color %d\n", getTeamColor());

// Settting team positions
Players teamA;
findTeam(teamA, debugFrame, groupedBlobs.team);
setTeamColor(getTeamColor() == Color::YELLOW ? Color::BLUE : Color::YELLOW);
Players teamB;
USE_PATTERN_FOR_ENEMIES ? findTeam(teamB, debugFrame, groupedBlobs.enemys) : findEnemys(teamB, debugFrame, groupedBlobs.enemys);
findTeam(teamB, debugFrame, groupedBlobs.enemies);
setTeamColor(getTeamColor() == Color::YELLOW ? Color::BLUE : Color::YELLOW);

Players allPlayers;
Expand All @@ -48,65 +47,85 @@ void PositionProcessing::matchBlobs(cv::Mat& debugFrame){
}

void PositionProcessing::findTeam(Players &players, cv::Mat& debugFrame, std::vector<Region> &teamRegions) {
players.clear();

std::bitset<MAX_PLAYERS> markedColors;
uint teamColor = static_cast<uint>(getTeamColor());
//printf("color Index : ");
for (Region &region : teamRegions) {
if (region.distance < blobMaxDist()) {
//int colorIndex = Utils::convertOldColorToNewColor(region.color);
int colorIndex = region.color;
//printf(" %d", colorIndex);
if (!Utils::isRobotColor(colorIndex)) {
// cor invalida
continue;
}

if (markedColors[size_t(colorIndex)]){
continue;
}

markedColors[size_t(colorIndex)] = true;
Blob b1, b2;
std::tie(b1, b2) = region.blobs;
Player robot((teamColor-1)*100 + static_cast<uint>(colorIndex) - Color::RED);
robot.team(teamColor);
Point newPositionInPixels = (b1.position + b2.position) * 0.5;
Point newPosition = Utils::convertPositionPixelToCm(newPositionInPixels);

Float newAngle = Utils::angle(b1.position, b2.position);

auto & playerPosVel = _kalmanFilterRobots[teamColor-2][robot.id()%100].update(newPosition.x,newPosition.y);

Geometry::PT filtPoint (playerPosVel(0, 0), playerPosVel(1, 0));
Geometry::PT PlayVel(playerPosVel(2, 0), playerPosVel(3, 0));

auto &playerRotVel = _dirFilteRobots[teamColor-2][robot.id()%100].update(std::cos(newAngle), std::sin(newAngle));
double filterDir = std::atan2(playerRotVel(1, 0), playerRotVel(0, 0));
robot.update(Point(filtPoint.x,filtPoint.y), filterDir);
players.push_back(robot);
cv::circle(debugFrame, Utils::convertPositionCmToPixel(Point(filtPoint.x,filtPoint.y)), 15, _colorCar[teamColor], 2, cv::LINE_AA);
cv::circle(debugFrame, Utils::convertPositionCmToPixel(Point(filtPoint.x,filtPoint.y)), 12, _colorCar[colorIndex], 2, cv::LINE_AA);
players.clear();
filterPattern(teamRegions);
uint teamColor = static_cast<uint>(getTeamColor());
for (Region &region : teamRegions) {
if(USE_OLD_PATTERN){

int colorIndex = region.blobs[1].color;

if (!Utils::isRobotColor(colorIndex)) {
// cor invalida
continue;
}

Blobs blobs = region.blobs;
Blob b1 = blobs[1], b2 = blobs[0];
Player robot((teamColor-1)*100 + static_cast<uint>(colorIndex) - Color::RED); // seta o id da robo na regiao
robot.team(teamColor);
Point newPositionInPixels = (b1.position + b2.position) * 0.5;
Point newPosition = Utils::convertPositionPixelToCm(newPositionInPixels);
Float newAngle = Utils::angle(b1.position, b2.position);

robot.update(Point(newPosition.x,newPosition.y), newAngle);
players.push_back(robot);
cv::circle(debugFrame, Utils::convertPositionCmToPixel(Point(newPosition.x,newPosition.y)), 15, _colorCar[teamColor], 2, cv::LINE_AA);
cv::circle(debugFrame, Utils::convertPositionCmToPixel(Point(newPosition.x,newPosition.y)), 12, _colorCar[colorIndex], 2, cv::LINE_AA);
}
else if(region.blobs.size() == 3) {
int firstSecondary = region.blobs[1].color;
int secondSecondary = region.blobs[2].color;
int colorIndex = ((teamColor) + (firstSecondary * MAX_ROBOTS) + (secondSecondary * MAX_ROBOTS * MAX_ROBOTS));

if (!Utils::isRobotColor(firstSecondary) || !Utils::isRobotColor(secondSecondary)) {
continue;
}
}

// markedColors[size_t(newId(colorIndex))] = true;
Blobs blobs = region.blobs;
Blob b1 = blobs[0], b2 = blobs[1], b3 = blobs[2];
if(newId(colorIndex) > 12)
continue;

Player robot(newId(colorIndex));

robot.team(teamColor);

cv::Point secondaryPosition = (b2.position + b3.position) * 0.5;
Point newPositionInPixels = (b1.position + secondaryPosition) * 0.5;
Point newPosition = Utils::convertPositionPixelToCm(newPositionInPixels);
Float newAngle = Utils::angle(secondaryPosition, b1.position);

auto & playerPosVel = _kalmanFilterRobots[teamColor-2][robot.id()].update(newPosition.x,newPosition.y);

Geometry::PT filtPoint (playerPosVel(0, 0), playerPosVel(1, 0));
Geometry::PT PlayVel(playerPosVel(2, 0), playerPosVel(3, 0));

auto &playerRotVel = _dirFilteRobots[teamColor-2][robot.id()%100].update(std::cos(newAngle), std::sin(newAngle));
double filterDir = std::atan2(playerRotVel(1, 0), playerRotVel(0, 0));

robot.update(Point(filtPoint.x,filtPoint.y), filterDir);
players.push_back(robot);

cv::circle(debugFrame, Utils::convertPositionCmToPixel(newPosition), 15, _colorCar[teamColor], 2, cv::LINE_AA);
cv::circle(debugFrame, Utils::convertPositionCmToPixel(newPosition), 12, _colorCar[teamColor], 2, cv::LINE_AA);
}
}
}

void PositionProcessing::findEnemys(Entities &players, cv::Mat& debugFrame, std::vector<Region> &enemyRegions) {

players.clear();

//std::bitset<MAX_PLAYERS> markedColors;
uint teamColor = this->_teamId == Color::YELLOW ? Color::BLUE : Color::YELLOW;


for (Region &region : enemyRegions) {
if (region.distance < blobMaxDist()) {
int colorIndex = region.color;
Blob b1, b2;
std::tie(b1, b2) = region.blobs;
int colorIndex = region.blobs[0].color;
Blobs blobs = region.blobs;
Blob b1 = blobs[0], b2 = blobs[1];
Player robot((teamColor-1)*100 + static_cast<uint>(colorIndex) - Color::RED);
robot.team(teamColor);
Point newPositionInPixels = b2.position ;
Expand All @@ -115,7 +134,7 @@ void PositionProcessing::findEnemys(Entities &players, cv::Mat& debugFrame, std:
// Debug
cv::circle(debugFrame, newPositionInPixels, 12, _colorCar[colorIndex], 2, cv::LINE_AA);

Float newAngle = Utils::angle(b2.position, b2.position);
Float newAngle = Utils::angle(b2.position, b1.position);
robot.update(newPosition, newAngle);
players.push_back(robot);
}
Expand Down Expand Up @@ -210,11 +229,101 @@ std::pair<PositionProcessing::Blob, PositionProcessing::NearestBlobInfo> Positio

result.distance = dMin;
result.teamIndex = team;
choosen.color = team;

return std::make_pair(choosen,result);

}

void PositionProcessing::filterPattern(Regions &regions) {

Regions f_regions;
// Sort regions by leftmost blob
for (auto &r : regions) {
if (r.blobs.size() < 3) {
continue;
}

Point b0 = r.blobs[0].position;
Point b1 = r.blobs[1].position;
Point b2 = r.blobs[2].position;

// Compute robot center:
Point robot = 0.5 * (b0 + 0.5 * (b1 + b2));

// Compute robot x and y axis vectors:
Point vx = 0.5 * (b1 + b2) - robot;
Point vy(-vx.y, vx.x);

// Compute b2 projection on vy
// Swap blobs if projection is positive (should be the opposite, but it worked this way!)
bool swap_blobs = ((b2.x-robot.x)*vy.x+(b2.y-robot.y)*vy.y>0);

if (swap_blobs) {
std::swap(r.blobs[1], r.blobs[2]);
}

f_regions.push_back(r);

// cv::Point b2 = (r.blobs[1].position + r.blobs[2].position) * 0.5;
//
//
// double angleThreshold = 10.0 * (M_PI / 180.0);
// double angleDiffTo180 = std::abs(std::abs(Utils::angle(b2, b1)) - M_PI);
//
// if(std::abs(Utils::angle(b2, b1)) < angleThreshold || angleDiffTo180 < angleThreshold) {
//
// if (r.blobs[0].position.x > (r.blobs[1].position.x + r.blobs[2].position.x) / 2) {
// if(r.blobs[1].position.y > r.blobs[2].position.y){
// std::swap(r.blobs[1], r.blobs[2]);
// }
// }else{
// if(r.blobs[1].position.y < r.blobs[2].position.y){
// std::swap(r.blobs[1], r.blobs[2]);
// }
// }
// }
// else if(r.blobs[0].position.y < (r.blobs[1].position.y + r.blobs[2].position.y)/2) // Primary blob on top
// {
// if(r.blobs[1].position.x > r.blobs[2].position.x)
// {
// std::swap(r.blobs[1], r.blobs[2]);
// }
// }
// else if(r.blobs[1].position.x < r.blobs[2].position.x) // Primary blob on bottom
// {
// std::swap(r.blobs[1], r.blobs[2]);
// }
// f_regions.push_back(r);
}
regions = f_regions;
}

void PositionProcessing::filterTeam(Regions &regions) {
std::unordered_map<int,Region> t_regions;
Regions r_regions;

for (auto &r : regions) {
int id = r.blobs[0].id;
if (t_regions.find(id) == t_regions.end()) { // Didn't found region of with given id
t_regions[id] = r; // Then, associate it with this region
} else {
Region &t_r = t_regions[r.blobs[0].id]; // Get already existing region of same id
t_r.blobs.push_back(r.blobs[1]); // Insert second secondary color
}
}

// Set all of the new merged regions into a vector
for (auto &r_t : t_regions) {
r_regions.push_back(r_t.second);
}
// Sort them by first blob id
std::sort(r_regions.begin(),r_regions.end());

// Update regions with filtered blobs
regions.assign(r_regions.begin(),r_regions.end());
}

PositionProcessing::FieldRegions PositionProcessing::pairBlobs() {
FieldRegions result;
std::pair<Blob, NearestBlobInfo> primary;
Expand All @@ -223,38 +332,44 @@ PositionProcessing::FieldRegions PositionProcessing::pairBlobs() {
for(int idColor = Color::RED ; idColor < Color::BROWN+1; idColor++) {
for(int i = 0 ; i < CLUSTERSPERCOLOR ; i++) {
if(blob[idColor][i].valid) {
current.blobs.clear();
primary = this->getNearestPrimary(blob[idColor][i]);
blob[idColor][i].color = idColor;

current.blobs = std::make_pair(blob[idColor][i], primary.first);
current.blobs.push_back(primary.first);
current.blobs.push_back(blob[idColor][i]);
current.team = primary.second.teamIndex;
current.color = idColor;

current.distance = primary.second.distance;
if (current.team == this->_teamId) result.team.push_back(current);
else if (USE_PATTERN_FOR_ENEMIES) result.enemys.push_back(current);
else if (USE_PATTERN_FOR_ENEMIES) result.enemies.push_back(current);
} else break;
}
}

if(!USE_PATTERN_FOR_ENEMIES && (result.enemys.empty() || result.enemys.size() < 3))
if(!USE_PATTERN_FOR_ENEMIES && (result.enemies.empty() || result.enemies.size() < 3))
{
int idColor = this->_teamId == Color::YELLOW ? Color::BLUE : Color::YELLOW;
int qnt = 0;
for(int i = 0 ; i < CLUSTERSPERCOLOR ; i++) {
if(blob[idColor][i].valid) {

current.blobs = std::make_pair(blob[idColor][i], blob[idColor][i]);
current.team = idColor;
current.color = Color::RED + qnt++;
current.distance = 0.0;
result.enemys.push_back(current);
}
int idColor = this->_teamId == Color::YELLOW ? Color::BLUE : Color::YELLOW;
int qnt = 0;
for(int i = 0 ; i < CLUSTERSPERCOLOR ; i++) {
if(blob[idColor][i].valid) {
current.blobs.clear();

blob[idColor][i].color = current.team;
current.blobs.push_back(blob[idColor][i]);
current.blobs.push_back(blob[idColor][i]);
current.team = idColor;
current.distance = 0.0;
result.enemies.push_back(current);
}
}
}
filterTeam(result.team);
filterTeam(result.enemies);

return result;
}


void PositionProcessing::setUp(std::string var, int value)
{
this->param[var] = value;
Expand Down Expand Up @@ -388,4 +503,20 @@ int PositionProcessing::blobMaxDist() {
return _blobMaxDist;
}

int PositionProcessing::newId(int oldId){
int id = 255;

auto it = std::find(idGenerated.begin(), idGenerated.end(), oldId);

if (it != idGenerated.end()){
id = std::distance(idGenerated.begin(), it);
} else {
it = std::find(idGenerated.begin(), idGenerated.end(), oldId-1);
if(it != idGenerated.end()){
id = std::distance(idGenerated.begin(), it);
}
}
return id;
}


Loading

0 comments on commit 8d41dcc

Please sign in to comment.