//Due to various information being needed to calculate the link it is necessary to create the temp experience
// 'currentExperience' and insert it into the experience map once a new experience becomes active
// the linkLastToCurrent performs the linking - with the linkExperience calling that and setting up a new temp
// experience
void linkLastToCurrent() //note:- trying char for now to try and cut memory problems
{
int currentMeanTime = (Map.currentExperienceStartTime + (nPgmTime/1000)) / 2;
if(Map.lastMatchedExperienceID != -1) //therefore not null
{
//time taken for transistion
char transitionTime = currentMeanTime - Map.lastExperienceMeanTime; //shouldnt be more than 128 seconds for transistion
//Set up pose info and copy data into
vector3D currentOdoPose;
memcpy(currentOdoPose, Map.currentExperience.odoPose,6); // may need to memcpy
vector3D lastOdoPose;
memcpy(lastOdoPose,Map.experienceMap[Map.lastMatchedExperienceID].odoPose,6); //same as above
//angle to current (angle = atan(y/x))
float angleToCurrent = getAngleDegrees((currentOdoPose.x - lastOdoPose.x),(currentOdoPose.y - lastOdoPose.y));
float translationAngle = lastOdoPose.theta - angleToCurrent;
//translation distance (in encoder clicks)
int translationDistance = getLength((currentOdoPose.x - lastOdoPose.x),(currentOdoPose.y - lastOdoPose.y));
//relative change in rotation
float rotation = getRotationDegrees(lastOdoPose.theta, currentOdoPose.theta);
//therefore has not been inserted onto experience map yet
if(Map.currentExperience.mapPose.x == -1) //null statement
{
vector3D lastMapPose;
vector3D newMapPose;
memcpy(lastMapPose, Map.experienceMap[Map.lastMatchedExperienceID].mapPose, 6);
float calcsAngle = lastMapPose.theta + translationAngle;
newMapPose.x = (lastMapPose.x + cosDegrees(calcsAngle) * translationDistance);
newMapPose.y = (lastMapPose.y + sinDegrees(calcsAngle) * translationDistance);
newMapPose.theta = (lastMapPose.theta + rotation);
memcpy(Map.currentExperience.mapPose, newMapPose, 6); //set up mapPose for current Experience
memcpy(Map.experienceMap[Map.currentExperience.ID], Map.currentExperience, 112); //put currentExperience on the map
}
char y;
int linkNumber;
for(y = 0; y<5; y++)
{
linkNumber = Map.experienceMap[Map.lastMatchedExperienceID].outLinks[y];
if(linkNumber != -1)
{
if(links[linkNumber].endExperienceID == Map.currentExperience.ID)
{
break; //using linkNumber
}
}
linkNumber = -1;
}
//if no link
if(linkNumber == -1)
{
//create new link with all data needed
experienceLink link;
link.startExperienceID = Map.lastMatchedExperienceID;
link.endExperienceID = Map.currentExperience.ID;
link.transitionTime = transitionTime;
link.translationAngle = translationAngle;
link.translationDistance = translationDistance;
link.rotation = rotation;
memcpy(links[nextLink],link,12);
setOutlinks(nextLink, Map.experienceMap[Map.lastMatchedExperienceID],Map.experienceMap[Map.currentExperience.ID]); //sets the outlinks and inlinks
nextLink++; //increment new link
}
//else if link existes, update
else
{
experienceLink previous;
memcpy(previous,links[linkNumber],12);
previous.transitionTime = (previous.transitionTime + transitionTime) / 2;
previous.translationAngle = (previous.translationAngle + translationAngle) / 2;
previous.translationDistance = (previous.translationDistance + translationDistance) / 2;
previous.rotation = (previous.rotation + rotation) / 2;
memcpy(links[linkNumber], previous,12);
}
}
//set current as previous experience
Map.lastMatchedExperienceID = Map.currentExperience.ID;
Map.lastExperienceMeanTime = currentMeanTime;
}
void Object::update()
{
shape.setPosition(getSfCoords(body->GetPosition()));
shape.setRotation(-getAngleDegrees(body->GetAngle()));
}
// Process an image containing a line and return the angle with respect to NAO.
double NaoVision::calculateAngleToBlackLine() {
// Convert image to gray and blur it.
cvtColor(src, src_gray, CV_BGR2GRAY);
blur(src_gray, src_gray, Size(3,3));
if(!local)
imshow("src", src);
Mat canny_output;
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
// Detect edges using canny.
Canny(src_gray, canny_output, thresh, thresh * 2, 3);
// Find contours.
findContours(canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
// Get the moments.
vector<Moments> mu(contours.size());
for(int i = 0; i < contours.size(); i++)
mu[i] = moments(contours[i], false);
// Get the mass centers.
vector<Point2f> mc( contours.size());
for(int i = 0; i < contours.size(); i++)
mc[i] = Point2f( mu[i].m10/mu[i].m00 , mu[i].m01/mu[i].m00);
// Eliminate contours without area.
contoursClean.clear();
int indMax = 0;
int lengthMax = 0;
for(int i = 0; i < contours.size(); i++) {
area = mu[i].m00;
length = arcLength(contours[i], true);
punto = mc[i];
if(area != 0 && length > 200 && punto.x > 0 && punto.y > 0)
contoursClean.push_back(contours.at(i));
}
if(contoursClean.size() != 0) {
// Get moments and mass for new vector.
vector<Moments> muClean(contoursClean.size());
for(int i = 0; i < contoursClean.size(); i++)
muClean[i] = moments(contoursClean[i], false);
// Get the mass centers.
vector<Point2f> mcClean( contoursClean.size());
for(int i = 0; i < contoursClean.size(); i++)
mcClean[i] = Point2f(muClean[i].m10/muClean[i].m00, muClean[i].m01/muClean[i].m00);
for(int i = 0; i < contoursClean.size(); i++) {
punto = mcClean[i];
length = arcLength(contoursClean[i], true);
}
// Find the longest.
for(int i = 0; i < contoursClean.size(); i++) {
length = arcLength(contoursClean[i], true);
lengthMax = arcLength(contoursClean[indMax], true);
if(i > 0) {
if(length > lengthMax)
indMax = i;
} else
indMax = 0;
}
// Draw contours.
Mat drawing = Mat::zeros(canny_output.size(), CV_8UC3);
Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255));
drawContours( drawing, contoursClean, indMax, color, 2, 8, hierarchy, 0, Point());
circle(drawing, mcClean[indMax], 4, color, 5, 8, 0 );
// Calculate the angle of the line.
angleToALine = getAngleDegrees(contoursClean[indMax], drawing);
puntoMax = mcClean[indMax];
lengthMax = arcLength(contoursClean[indMax], true);
// Show in a window.
if(!local) {
namedWindow("Contours", CV_WINDOW_AUTOSIZE);
imshow("Contours", drawing);
// Draw grid.
line(drawing, Point(260,0), Point(260, drawing.rows), Scalar(255,255,255));
line(drawing, Point(umbral,0), Point(umbral, drawing.rows), Scalar(255,255,255));
line(drawing, Point((drawing.cols/2),0), Point((drawing.cols/2), drawing.rows), Scalar(255,255,255));
line(drawing, Point(0,120), Point(320,120), Scalar(255,255,255));
imshow("Contours", drawing);
}
}
else { // Go straight.
angleToALine = 90.0;
}
return angleToALine;
}
