Skip to content

Commit 8d41dcc

Browse files
ersaraujoacrucha
ersaraujo
and
acrucha
authored
feat(PositionProcessing): Detect new pattern (#37)
* 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 <mcaa@cin.ufpe.br> --------- Co-authored-by: acrucha <mcaa@cin.ufpe.br>
1 parent 2157385 commit 8d41dcc

14 files changed

Lines changed: 293 additions & 85 deletions

File tree

‎client/python/receiver.py‎

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -22,13 +22,11 @@ def __init__(self,
2222
self.vision_ip = vision_ip
2323
self.vision_port = vision_port
2424

25-
self.vision_sock = socket.socket(
26-
socket.AF_INET, socket.SOCK_DGRAM)
25+
self.vision_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
2726
self.vision_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
28-
self.vision_sock.setsockopt(socket.IPPROTO_IP,
29-
socket.IP_MULTICAST_TTL, 128)
30-
self.vision_sock.setsockopt(socket.IPPROTO_IP,
31-
socket.IP_MULTICAST_LOOP, 1)
27+
self.vision_sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 128)
28+
self.vision_sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1)
29+
self.vision_sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, struct.pack("=4sl", socket.inet_aton(self.vision_ip), socket.INADDR_ANY))
3230
self.vision_sock.bind((self.vision_ip, self.vision_port))
3331

3432
# self.vision_sock.setblocking(True)

‎src/Network/visionServer/server.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@ void VisionServer::send(std::vector<Entity> &entities) {
4343
} else{
4444
if((int)entities[i].team() == Color::YELLOW){
4545
SSL_DetectionRobot *robot = frame->mutable_robots_yellow()->Add();
46-
robot->set_robot_id(entities[i].id()-200);
46+
robot->set_robot_id(entities[i].id());
4747
robot->set_x(entities[i].position().x * 10);
4848
robot->set_y(entities[i].position().y * 10);
4949
robot->set_orientation(entities[i].angle());
@@ -52,7 +52,7 @@ void VisionServer::send(std::vector<Entity> &entities) {
5252
robot->set_pixel_y(robot->y()*100);
5353
}else if((int)entities[i].team() == Color::BLUE){
5454
SSL_DetectionRobot *robot = frame->mutable_robots_blue()->Add();
55-
robot->set_robot_id(entities[i].id()-100);
55+
robot->set_robot_id(entities[i].id());
5656
robot->set_x(entities[i].position().x * 10);
5757
robot->set_y(entities[i].position().y * 10);
5858
robot->set_orientation(entities[i].angle());

‎src/Vision/PositionProcessing/BlobDetection.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -108,6 +108,7 @@ void BlobDetection::findBlobs(cv::Mat& debugFrame) {
108108
blob[cor][_clusterCount[cor]].position.y = r.sumX / this->_runs[i][j].areaBlob;
109109
blob[cor][_clusterCount[cor]].area = r.areaBlob;
110110
blob[cor][_clusterCount[cor]].valid = true;
111+
blob[cor][_clusterCount[cor]].id = _clusterCount[cor];
111112
blob[cor][_clusterCount[cor] + 1].valid = false;
112113
_clusterCount[cor]++;
113114

‎src/Vision/PositionProcessing/PositionProcessing.cpp‎

Lines changed: 196 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
#include "PositionProcessing.h"
2-
2+
#include "Utils/EnumsAndConstants.h"
33

44
void PositionProcessing::saveXML()
55
{
@@ -26,14 +26,13 @@ void PositionProcessing::matchBlobs(cv::Mat& debugFrame){
2626
static Players empty_players;
2727

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

3130
// Settting team positions
3231
Players teamA;
3332
findTeam(teamA, debugFrame, groupedBlobs.team);
3433
setTeamColor(getTeamColor() == Color::YELLOW ? Color::BLUE : Color::YELLOW);
3534
Players teamB;
36-
USE_PATTERN_FOR_ENEMIES ? findTeam(teamB, debugFrame, groupedBlobs.enemys) : findEnemys(teamB, debugFrame, groupedBlobs.enemys);
35+
findTeam(teamB, debugFrame, groupedBlobs.enemies);
3736
setTeamColor(getTeamColor() == Color::YELLOW ? Color::BLUE : Color::YELLOW);
3837

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

5049
void PositionProcessing::findTeam(Players &players, cv::Mat& debugFrame, std::vector<Region> &teamRegions) {
51-
players.clear();
52-
53-
std::bitset<MAX_PLAYERS> markedColors;
54-
uint teamColor = static_cast<uint>(getTeamColor());
55-
//printf("color Index : ");
56-
for (Region &region : teamRegions) {
57-
if (region.distance < blobMaxDist()) {
58-
//int colorIndex = Utils::convertOldColorToNewColor(region.color);
59-
int colorIndex = region.color;
60-
//printf(" %d", colorIndex);
61-
if (!Utils::isRobotColor(colorIndex)) {
62-
// cor invalida
63-
continue;
64-
}
65-
66-
if (markedColors[size_t(colorIndex)]){
67-
continue;
68-
}
69-
70-
markedColors[size_t(colorIndex)] = true;
71-
Blob b1, b2;
72-
std::tie(b1, b2) = region.blobs;
73-
Player robot((teamColor-1)*100 + static_cast<uint>(colorIndex) - Color::RED);
74-
robot.team(teamColor);
75-
Point newPositionInPixels = (b1.position + b2.position) * 0.5;
76-
Point newPosition = Utils::convertPositionPixelToCm(newPositionInPixels);
77-
78-
Float newAngle = Utils::angle(b1.position, b2.position);
7950

80-
auto & playerPosVel = _kalmanFilterRobots[teamColor-2][robot.id()%100].update(newPosition.x,newPosition.y);
81-
82-
Geometry::PT filtPoint (playerPosVel(0, 0), playerPosVel(1, 0));
83-
Geometry::PT PlayVel(playerPosVel(2, 0), playerPosVel(3, 0));
84-
85-
auto &playerRotVel = _dirFilteRobots[teamColor-2][robot.id()%100].update(std::cos(newAngle), std::sin(newAngle));
86-
double filterDir = std::atan2(playerRotVel(1, 0), playerRotVel(0, 0));
87-
robot.update(Point(filtPoint.x,filtPoint.y), filterDir);
88-
players.push_back(robot);
89-
cv::circle(debugFrame, Utils::convertPositionCmToPixel(Point(filtPoint.x,filtPoint.y)), 15, _colorCar[teamColor], 2, cv::LINE_AA);
90-
cv::circle(debugFrame, Utils::convertPositionCmToPixel(Point(filtPoint.x,filtPoint.y)), 12, _colorCar[colorIndex], 2, cv::LINE_AA);
51+
players.clear();
52+
filterPattern(teamRegions);
53+
uint teamColor = static_cast<uint>(getTeamColor());
54+
for (Region &region : teamRegions) {
55+
if(USE_OLD_PATTERN){
56+
57+
int colorIndex = region.blobs[1].color;
58+
59+
if (!Utils::isRobotColor(colorIndex)) {
60+
// cor invalida
61+
continue;
62+
}
9163

64+
Blobs blobs = region.blobs;
65+
Blob b1 = blobs[1], b2 = blobs[0];
66+
Player robot((teamColor-1)*100 + static_cast<uint>(colorIndex) - Color::RED); // seta o id da robo na regiao
67+
robot.team(teamColor);
68+
Point newPositionInPixels = (b1.position + b2.position) * 0.5;
69+
Point newPosition = Utils::convertPositionPixelToCm(newPositionInPixels);
70+
Float newAngle = Utils::angle(b1.position, b2.position);
71+
72+
robot.update(Point(newPosition.x,newPosition.y), newAngle);
73+
players.push_back(robot);
74+
cv::circle(debugFrame, Utils::convertPositionCmToPixel(Point(newPosition.x,newPosition.y)), 15, _colorCar[teamColor], 2, cv::LINE_AA);
75+
cv::circle(debugFrame, Utils::convertPositionCmToPixel(Point(newPosition.x,newPosition.y)), 12, _colorCar[colorIndex], 2, cv::LINE_AA);
76+
}
77+
else if(region.blobs.size() == 3) {
78+
int firstSecondary = region.blobs[1].color;
79+
int secondSecondary = region.blobs[2].color;
80+
int colorIndex = ((teamColor) + (firstSecondary * MAX_ROBOTS) + (secondSecondary * MAX_ROBOTS * MAX_ROBOTS));
81+
82+
if (!Utils::isRobotColor(firstSecondary) || !Utils::isRobotColor(secondSecondary)) {
83+
continue;
9284
}
93-
}
9485

86+
// markedColors[size_t(newId(colorIndex))] = true;
87+
Blobs blobs = region.blobs;
88+
Blob b1 = blobs[0], b2 = blobs[1], b3 = blobs[2];
89+
if(newId(colorIndex) > 12)
90+
continue;
91+
92+
Player robot(newId(colorIndex));
93+
94+
robot.team(teamColor);
95+
96+
cv::Point secondaryPosition = (b2.position + b3.position) * 0.5;
97+
Point newPositionInPixels = (b1.position + secondaryPosition) * 0.5;
98+
Point newPosition = Utils::convertPositionPixelToCm(newPositionInPixels);
99+
Float newAngle = Utils::angle(secondaryPosition, b1.position);
100+
101+
auto & playerPosVel = _kalmanFilterRobots[teamColor-2][robot.id()].update(newPosition.x,newPosition.y);
102+
103+
Geometry::PT filtPoint (playerPosVel(0, 0), playerPosVel(1, 0));
104+
Geometry::PT PlayVel(playerPosVel(2, 0), playerPosVel(3, 0));
105+
106+
auto &playerRotVel = _dirFilteRobots[teamColor-2][robot.id()%100].update(std::cos(newAngle), std::sin(newAngle));
107+
double filterDir = std::atan2(playerRotVel(1, 0), playerRotVel(0, 0));
108+
109+
robot.update(Point(filtPoint.x,filtPoint.y), filterDir);
110+
players.push_back(robot);
111+
112+
cv::circle(debugFrame, Utils::convertPositionCmToPixel(newPosition), 15, _colorCar[teamColor], 2, cv::LINE_AA);
113+
cv::circle(debugFrame, Utils::convertPositionCmToPixel(newPosition), 12, _colorCar[teamColor], 2, cv::LINE_AA);
114+
}
115+
}
95116
}
96117

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

99120
players.clear();
100121

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

104-
105124
for (Region &region : enemyRegions) {
106125
if (region.distance < blobMaxDist()) {
107-
int colorIndex = region.color;
108-
Blob b1, b2;
109-
std::tie(b1, b2) = region.blobs;
126+
int colorIndex = region.blobs[0].color;
127+
Blobs blobs = region.blobs;
128+
Blob b1 = blobs[0], b2 = blobs[1];
110129
Player robot((teamColor-1)*100 + static_cast<uint>(colorIndex) - Color::RED);
111130
robot.team(teamColor);
112131
Point newPositionInPixels = b2.position ;
@@ -115,7 +134,7 @@ void PositionProcessing::findEnemys(Entities &players, cv::Mat& debugFrame, std:
115134
// Debug
116135
cv::circle(debugFrame, newPositionInPixels, 12, _colorCar[colorIndex], 2, cv::LINE_AA);
117136

118-
Float newAngle = Utils::angle(b2.position, b2.position);
137+
Float newAngle = Utils::angle(b2.position, b1.position);
119138
robot.update(newPosition, newAngle);
120139
players.push_back(robot);
121140
}
@@ -210,11 +229,101 @@ std::pair<PositionProcessing::Blob, PositionProcessing::NearestBlobInfo> Positio
210229

211230
result.distance = dMin;
212231
result.teamIndex = team;
232+
choosen.color = team;
213233

214234
return std::make_pair(choosen,result);
215235

216236
}
217237

238+
void PositionProcessing::filterPattern(Regions &regions) {
239+
240+
Regions f_regions;
241+
// Sort regions by leftmost blob
242+
for (auto &r : regions) {
243+
if (r.blobs.size() < 3) {
244+
continue;
245+
}
246+
247+
Point b0 = r.blobs[0].position;
248+
Point b1 = r.blobs[1].position;
249+
Point b2 = r.blobs[2].position;
250+
251+
// Compute robot center:
252+
Point robot = 0.5 * (b0 + 0.5 * (b1 + b2));
253+
254+
// Compute robot x and y axis vectors:
255+
Point vx = 0.5 * (b1 + b2) - robot;
256+
Point vy(-vx.y, vx.x);
257+
258+
// Compute b2 projection on vy
259+
// Swap blobs if projection is positive (should be the opposite, but it worked this way!)
260+
bool swap_blobs = ((b2.x-robot.x)*vy.x+(b2.y-robot.y)*vy.y>0);
261+
262+
if (swap_blobs) {
263+
std::swap(r.blobs[1], r.blobs[2]);
264+
}
265+
266+
f_regions.push_back(r);
267+
268+
// cv::Point b2 = (r.blobs[1].position + r.blobs[2].position) * 0.5;
269+
//
270+
//
271+
// double angleThreshold = 10.0 * (M_PI / 180.0);
272+
// double angleDiffTo180 = std::abs(std::abs(Utils::angle(b2, b1)) - M_PI);
273+
//
274+
// if(std::abs(Utils::angle(b2, b1)) < angleThreshold || angleDiffTo180 < angleThreshold) {
275+
//
276+
// if (r.blobs[0].position.x > (r.blobs[1].position.x + r.blobs[2].position.x) / 2) {
277+
// if(r.blobs[1].position.y > r.blobs[2].position.y){
278+
// std::swap(r.blobs[1], r.blobs[2]);
279+
// }
280+
// }else{
281+
// if(r.blobs[1].position.y < r.blobs[2].position.y){
282+
// std::swap(r.blobs[1], r.blobs[2]);
283+
// }
284+
// }
285+
// }
286+
// else if(r.blobs[0].position.y < (r.blobs[1].position.y + r.blobs[2].position.y)/2) // Primary blob on top
287+
// {
288+
// if(r.blobs[1].position.x > r.blobs[2].position.x)
289+
// {
290+
// std::swap(r.blobs[1], r.blobs[2]);
291+
// }
292+
// }
293+
// else if(r.blobs[1].position.x < r.blobs[2].position.x) // Primary blob on bottom
294+
// {
295+
// std::swap(r.blobs[1], r.blobs[2]);
296+
// }
297+
// f_regions.push_back(r);
298+
}
299+
regions = f_regions;
300+
}
301+
302+
void PositionProcessing::filterTeam(Regions &regions) {
303+
std::unordered_map<int,Region> t_regions;
304+
Regions r_regions;
305+
306+
for (auto &r : regions) {
307+
int id = r.blobs[0].id;
308+
if (t_regions.find(id) == t_regions.end()) { // Didn't found region of with given id
309+
t_regions[id] = r; // Then, associate it with this region
310+
} else {
311+
Region &t_r = t_regions[r.blobs[0].id]; // Get already existing region of same id
312+
t_r.blobs.push_back(r.blobs[1]); // Insert second secondary color
313+
}
314+
}
315+
316+
// Set all of the new merged regions into a vector
317+
for (auto &r_t : t_regions) {
318+
r_regions.push_back(r_t.second);
319+
}
320+
// Sort them by first blob id
321+
std::sort(r_regions.begin(),r_regions.end());
322+
323+
// Update regions with filtered blobs
324+
regions.assign(r_regions.begin(),r_regions.end());
325+
}
326+
218327
PositionProcessing::FieldRegions PositionProcessing::pairBlobs() {
219328
FieldRegions result;
220329
std::pair<Blob, NearestBlobInfo> primary;
@@ -223,38 +332,44 @@ PositionProcessing::FieldRegions PositionProcessing::pairBlobs() {
223332
for(int idColor = Color::RED ; idColor < Color::BROWN+1; idColor++) {
224333
for(int i = 0 ; i < CLUSTERSPERCOLOR ; i++) {
225334
if(blob[idColor][i].valid) {
335+
current.blobs.clear();
226336
primary = this->getNearestPrimary(blob[idColor][i]);
337+
blob[idColor][i].color = idColor;
227338

228-
current.blobs = std::make_pair(blob[idColor][i], primary.first);
339+
current.blobs.push_back(primary.first);
340+
current.blobs.push_back(blob[idColor][i]);
229341
current.team = primary.second.teamIndex;
230-
current.color = idColor;
231342

232343
current.distance = primary.second.distance;
233344
if (current.team == this->_teamId) result.team.push_back(current);
234-
else if (USE_PATTERN_FOR_ENEMIES) result.enemys.push_back(current);
345+
else if (USE_PATTERN_FOR_ENEMIES) result.enemies.push_back(current);
235346
} else break;
236347
}
237348
}
238349

239-
if(!USE_PATTERN_FOR_ENEMIES && (result.enemys.empty() || result.enemys.size() < 3))
350+
if(!USE_PATTERN_FOR_ENEMIES && (result.enemies.empty() || result.enemies.size() < 3))
240351
{
241-
int idColor = this->_teamId == Color::YELLOW ? Color::BLUE : Color::YELLOW;
242-
int qnt = 0;
243-
for(int i = 0 ; i < CLUSTERSPERCOLOR ; i++) {
244-
if(blob[idColor][i].valid) {
245-
246-
current.blobs = std::make_pair(blob[idColor][i], blob[idColor][i]);
247-
current.team = idColor;
248-
current.color = Color::RED + qnt++;
249-
current.distance = 0.0;
250-
result.enemys.push_back(current);
251-
}
352+
int idColor = this->_teamId == Color::YELLOW ? Color::BLUE : Color::YELLOW;
353+
int qnt = 0;
354+
for(int i = 0 ; i < CLUSTERSPERCOLOR ; i++) {
355+
if(blob[idColor][i].valid) {
356+
current.blobs.clear();
357+
358+
blob[idColor][i].color = current.team;
359+
current.blobs.push_back(blob[idColor][i]);
360+
current.blobs.push_back(blob[idColor][i]);
361+
current.team = idColor;
362+
current.distance = 0.0;
363+
result.enemies.push_back(current);
252364
}
365+
}
253366
}
367+
filterTeam(result.team);
368+
filterTeam(result.enemies);
369+
254370
return result;
255371
}
256372

257-
258373
void PositionProcessing::setUp(std::string var, int value)
259374
{
260375
this->param[var] = value;
@@ -388,4 +503,20 @@ int PositionProcessing::blobMaxDist() {
388503
return _blobMaxDist;
389504
}
390505

506+
int PositionProcessing::newId(int oldId){
507+
int id = 255;
508+
509+
auto it = std::find(idGenerated.begin(), idGenerated.end(), oldId);
510+
511+
if (it != idGenerated.end()){
512+
id = std::distance(idGenerated.begin(), it);
513+
} else {
514+
it = std::find(idGenerated.begin(), idGenerated.end(), oldId-1);
515+
if(it != idGenerated.end()){
516+
id = std::distance(idGenerated.begin(), it);
517+
}
518+
}
519+
return id;
520+
}
521+
391522

0 commit comments

Comments
 (0)