Point LineSegment::closestPointOnSegmentTo(Point p)
{
float top = (p.x - p1.x) * (p2.x - p1.x) + (p.y - p1.y)*(p2.y - p1.y);
float bottom = distanceBetweenPoints(p2, p1);
bottom = bottom * bottom;
float u = top / bottom;
float x = p1.x + u * (p2.x - p1.x);
float y = p1.y + u * (p2.y - p1.y);
return Point(x, y);
}
void LineSegment::init(int x1, int y1, int x2, int y2)
{
this->p1 = Point(x1, y1);
this->p2 = Point(x2, y2);
if (p2.x - p1.x == 0)
this->slope = 0.00000000001;
else
this->slope = (float) (p2.y - p1.y) / (float) (p2.x - p1.x);
this->length = distanceBetweenPoints(p1, p2);
this->angle = angleBetweenPoints(p1, p2);
}
/**
* Publishes the total distance of the remaining path
*/
double PathHandler::getPathLength()
{
geometry_msgs::Point p;
p.x = mRobotPosition.getOrigin().getX();
p.y = mRobotPosition.getOrigin().getY();
p.z = mRobotPosition.getOrigin().getZ();
double length = 0.f;
for (uint32_t i = mCurrentPathIndex + 1; i < mPath.size(); i++)
{
length += distanceBetweenPoints(p, mPath[i].position);
p = mPath[i].position;
}
return length;
}
float SpaceOrientation:: computeDistance(Object object)
{
//computePointInRobotFrameGivenPointOnImage(object.getImage().getCenter());
cv::Point2d one1 = object.getPositionInRobotFrame();
// // To correct for grasping, let's update the position where we need to go:
// one1.x += LATERAL_DISTANCE_CORRECTION_FOR_HAND_GRASPING;
// one1.y += FORWARD_DISTANCE_CORRECTION_FOR_HAND_GRASPING;
// // What would happen when two.y gets negative? test it.
std::vector<float> nao = getNaoPositionInRobotFrame();
cv::Point2d two1(nao[1], nao[0]);
return distanceBetweenPoints(one1, two1);
}
void Curve::AddTranslation(int x, int y, ofVec3f _draggedPixelVector)
{
ofPoint tmp;
tmp.x = x - this->getTranslation().x;
tmp.y = y - this->getTranslation().y;
ofPoint closestPoint = curve.getClosestPoint(tmp);
ofPoint * inersected = intersectsPoint(tmp.x, tmp.y);
if (inersected != nullptr)
{
inersected->x = x - this->getTranslation().x;
inersected->y = y - this->getTranslation().y;
inersected = nullptr;
}
else if (distanceBetweenPoints(tmp.x, tmp.y, closestPoint.x, closestPoint.y) < 10)
Object2D::AddTranslation(x, y, _draggedPixelVector);
}
void positionCallback(const nav_msgs::OdometryConstPtr& msg)
{
std::lock_guard<std::mutex> lock(current_mutex);
geometry_msgs::PointStamped cur = current_waypoint;
cur.point.x -= map_origin.x;
cur.point.y -= map_origin.y;
if (distanceBetweenPoints(msg->pose.pose.position, cur.point) < 1.0)
{
// advance to next waypoint.
current_waypoint = waypoints.front();
if (waypoints.size() > 1)
{
waypoints.erase(waypoints.begin());
std::cerr << "Waypoint Source moving to next" << std::endl;
}
}
}
void Objects::processMonsterVsPacketCollision( )
{
auto monster = m_vupMonsters.begin( );
while( monster != m_vupMonsters.end( ) )
{
hgeVector monsterPos = ( *monster )->getPosition( );
auto packet = m_vupPackets.begin( );
while( packet != m_vupPackets.end( ) )
{
hgeVector packetPos = ( *packet )->getPosition( );
if( distanceBetweenPoints( monsterPos, packetPos ) < MONSTER_RADIUS + PACKET_RADIUS )
{
float monsterHealth = ( *monster )->getHealth( );
( *monster )->setHealth( monsterHealth - ( *monster )->getArmor( ) * ( *packet )->getDamage( ) );
( *packet ).reset( nullptr );
packet = m_vupPackets.erase( packet );
if( packet == m_vupPackets.end( ) )
{
break;
}
}
packet++;
}
if( ( *monster )->getHealth( ) <= 0.0f )
{
countDeadMonsters( ( *monster )->getType( ) );
monster = m_vupMonsters.erase( monster );
if( monster == m_vupMonsters.end( ) )
{
break;
}
}
else
{
monster++;
}
}
}
void Objects::processMonsterVsTankCollision( )
{
auto monster = m_vupMonsters.begin( );
hgeVector tankPos = m_upTank->getPosition( );
while( monster != m_vupMonsters.end( ) )
{
hgeVector monsterPos = ( *monster )->getPosition( );
if( distanceBetweenPoints( monsterPos, tankPos ) < TANK_RADIUS + MONSTER_RADIUS )
{
float tankHealth = m_upTank->getHealth( );
m_upTank->setHealth( tankHealth - m_upTank->getArmor( ) * ( *monster )->getDamage( ) );
( *monster )->setPosition( monsterPos - 10.0f * ( *monster )->getDirection( ) );
if( m_upTank->getHealth( ) < 0.0f )
{
attempts--;
return;
}
}
monster++;
}
}
KDvoid AnalogStick::repositionBarWithPoint ( const CCPoint& tPoint )
{
const CCSize tSize = this->getContentSize ( );
// Rotation
KDfloat fRadians = vectorToRadians ( m_tPressedVector );
KDfloat fDegrees = radiansToDegrees ( fRadians );
m_pBar->setRotation ( fDegrees );
CCSpriteFrameCache* pCache = CCSpriteFrameCache::sharedSpriteFrameCache ( );
m_pBar->setDisplayFrame ( pCache->spriteFrameByName ( "analog_bar.png" ) );
CCRect tBarTexRect = m_pBar->getTextureRect ( );
KDfloat fDistFromCenter = distanceBetweenPoints ( tPoint, this->getPosition ( ) );
KDfloat fSizeMod = fDistFromCenter / tSize.cx;
KDfloat fOldHeight = tBarTexRect.size.cy;
KDfloat fNewHeight = fOldHeight * fSizeMod * 5;
// Custom fixes
if ( fNewHeight < 100 )
{
fNewHeight = 100.0f;
}
if ( fDistFromCenter < 3 )
{
fNewHeight = 0.0f;
}
m_pBar->setTextureRect ( CCRect ( tBarTexRect.origin.x, tBarTexRect.origin.y + ( fOldHeight - fNewHeight ), tBarTexRect.size.cx, fNewHeight ) );
m_pBar->setAnchorPoint ( ccp ( 0.5f, 0 ) );
CCPoint tDirectionVector = radiansToVector ( fRadians - KD_PI_F / 2 );
m_pBar->setPosition ( ccp ( tSize.cx / 2 + tDirectionVector.x * tSize.cx / 4, tSize.cy / 2 + tDirectionVector.y * tSize.cy / 4 ) );
}
bool CCTapGestureRecognizer::ccTouchBegan(CCTouch * pTouch, CCEvent * pEvent)
{
if (isRecognizing && taps==0) {
stopGestureRecognition();
return false;
}
initialPosition = pTouch->getLocation();
if (!isPositionBetweenBounds(initialPosition)) return false;
CCTime::gettimeofdayCocos2d(&startTime, NULL);
if (taps>0 && taps<(int)numberOfTapsRequired) {
float distance = distanceBetweenPoints(finalPosition, initialPosition); //distance between taps
double duration = CCTime::timersubCocos2d(&endTime, &startTime); //duration between taps
if (duration>kTapMaxDurationBetweenTaps || distance>kTapMaxDistanceBetweenTaps) {
stopGestureRecognition();
}
}
isRecognizing = true;
return true;
}
void bSplineMotionUCompute(void) {
PidMotion* pidMotion = getPidMotion();
PidComputationValues* computationValues = &(pidMotion->computationValues);
PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
BSplineCurve* curve = &(motionDefinition->curve);
float pidTime = computationValues->pidTime;
MotionInstruction* thetaInst = &(motionDefinition->inst[THETA]);
float normalPosition = computeNormalPosition(thetaInst, pidTime);
// Computes the time of the bSpline in [0.00, 1.00]
float bSplineTime = computeBSplineTimeAtDistance(curve, normalPosition);
Point normalPoint;
// Computes the normal Point where the robot must be
computeBSplinePoint(curve, bSplineTime, &normalPoint);
// Convert normalPoint into mm space
RobotKinematics* robotKinematics = getRobotKinematics();
float wheelAverageLength = robotKinematics->wheelAverageLengthForOnePulse;
normalPoint.x = normalPoint.x * wheelAverageLength;
normalPoint.y = normalPoint.y * wheelAverageLength;
Position* robotPosition = getPosition();
Point robotPoint = robotPosition->pos;
// GET PID
unsigned pidIndex = getIndexOfPid(THETA, thetaInst->pidType);
unsigned char rollingTestMode = getRollingTestMode();
PidParameter* pidParameter = getPidParameter(pidIndex, rollingTestMode);
// ALPHA
PidMotionError* alphaMotionError = &(computationValues->errors[ALPHA]);
float normalAlpha = computeBSplineOrientationWithDerivative(curve, bSplineTime);
float realAlpha = robotPosition->orientation;
// backward management
if (curve->backward) {
realAlpha += PI;
}
float alphaError = (normalAlpha - realAlpha);
// restriction to [-PI, PI]
alphaError = mod2PI(alphaError);
// Convert angleError into pulse equivalent
float wheelsDistanceFromCenter = getWheelsDistanceFromCenter(robotKinematics);
float alphaPulseError = (-wheelsDistanceFromCenter * alphaError) / wheelAverageLength;
// THETA
PidMotionError* thetaMotionError = &(computationValues->errors[THETA]);
// thetaError must be in Pulse and not in MM
float thetaError = distanceBetweenPoints(&robotPoint, &normalPoint) / wheelAverageLength;
float thetaAngle = angleOfVector(&robotPoint, &normalPoint);
if (curve->backward) {
thetaAngle += PI;
}
float alphaAndThetaDiff = thetaAngle - normalAlpha;
// restriction to [-PI, PI]
alphaAndThetaDiff = mod2PI(alphaAndThetaDiff);
float cosAlphaAndThetaDiff = cosf(alphaAndThetaDiff);
float thetaErrorWithCos = thetaError * cosAlphaAndThetaDiff;
float normalSpeed = computeNormalSpeed(thetaInst, pidTime);
float thetaU = computePidCorrection(thetaMotionError, pidParameter, normalSpeed, thetaErrorWithCos);
PidCurrentValues* thetaCurrentValues = &(computationValues->currentValues[THETA]);
thetaCurrentValues->u = thetaU;
// ALPHA CORRECTION
alphaPulseError *= 5.0f;
float alphaCorrection = -0.00050f * normalSpeed * thetaError * (alphaAndThetaDiff);
// float alphaCorrection = 0.0f;
alphaPulseError += alphaCorrection;
float alphaU = computePidCorrection(alphaMotionError, pidParameter, 0, alphaPulseError);
PidCurrentValues* alphaCurrentValues = &(computationValues->currentValues[ALPHA]);
alphaCurrentValues->u = alphaU;
// LOG
OutputStream* out = getDebugOutputStreamLogger();
// appendStringAndDecf(out, "pt=", pidTime);
appendStringAndDecf(out, ",t=", bSplineTime);
// Normal Position
appendStringAndDecf(out, ",nx=", normalPoint.x);
appendStringAndDecf(out, ",ny=", normalPoint.y);
appendStringAndDecf(out, ",na=", normalAlpha);
// Real position
appendStringAndDecf(out, ",rx=", robotPoint.x);
appendStringAndDecf(out, ",ry=", robotPoint.y);
appendStringAndDecf(out, ",ra=", realAlpha);
// ALPHA
appendStringAndDecf(out, ",ta=", thetaAngle);
appendStringAndDecf(out, ",ae=", alphaError);
//appendStringAndDecf(out, ",atdiff=", alphaAndThetaDiff);
//appendStringAndDecf(out, ",catdiff=", cosAlphaAndThetaDiff);
appendStringAndDecf(out, ",au=", alphaU);
appendStringAndDecf(out, ",ac=", alphaCorrection);
// THETA
// appendString(out, ",np=");
// appendDecf(out, normalPosition);
appendStringAndDecf(out, ",te=", thetaError);
appendStringAndDecf(out, ",tec=", thetaErrorWithCos);
appendStringAndDecf(out, ",tu=", thetaU);
appendCRLF(out);
}
void boatUpdate(boat b, int keepDir, double timedif)
{
double anchorRatio;
if (b->extra->timeStuckLeft > 0) { /*Se o barco esta encalhado */
b->extra->timeStuckLeft -= timedif;
b->tex.color = b->extra->color / 2;
/* Torna o bote mais escuro - note que os valores para R e G sempre sao pares para lidar com isso */
if (b->extra->timeStuckLeft <= 0) {
if (b->extra->life >= 0) {
b->extra->timeStuckLeft = 0;
b->pos = b->extra->respawnPoint;
b->vel = vectorCreate(0, 0);
b->acc = vectorCreate(0, 0);
b->dir = PI/2;
b->tex.color = b->extra->color;
/* Retornando a cor original, ja que o bote muda de cor quando encalhado */
} else
b->toBeRemoved = 1;
}
return;
}
/*Verificando se ja deve ganahr uma vida nova*/
if (b->extra->points >=
b->extra->extraLivesCount * b->extra->extraLifeScore) {
b->extra->life++;
b->extra->extraLivesCount++;
}
/*Le teclado*/
boatReadKeyboard(b);
if (b->extra->isAccel && !b->extra->isAnchored) {
b->acc.x = -b->extra->accel * cos(b->dir) * b->extra->isAccel;
b->acc.y = -b->extra->accel * sin(b->dir) * b->extra->isAccel;
} else
b->acc = vectorCreate(0, 0);
if (b->extra->isAnchored)
anchorRatio = b->extra->anchorMultiplier;
else
anchorRatio = 1;
b->acc.x = b->acc.x - b->vel.x * b->extra->friction * anchorRatio;
b->acc.y = b->acc.y - b->vel.y * b->extra->friction * anchorRatio;
b->vel = vectorSum(b->vel, vectorMulDouble(b->acc, timedif));
b->pos = vectorSum(b->pos, vectorMulDouble(b->vel, timedif));
if (!b->extra->isAnchored)
b->dir += b->extra->isTurning * b->extra->turnRate * timedif;
objectQuadUpdate(b);
/*Resgate de pessoas*/
if (b->extra->isAnchored
&& distanceBetweenPoints(b->pos,
shipPos) <= b->extra->unloadDistance) {
if (b->extra->personList != NULL) {
if (b->extra->unloadTimeLeft <= 0.0) {
debugDouble("b->extra->unloadtimeleft",
b->extra->unloadTimeLeft);
rescuePerson(b);
b->extra->unloadTimeLeft = b->extra->unloadTime;
} else
b->extra->unloadTimeLeft =
b->extra->unloadTimeLeft - timedif;
}
} else
b->extra->unloadTimeLeft = b->extra->unloadTime;
/*Fala o que precisa ser mostrado no display*/
statusReport(b->extra->player, b->extra->name, b->extra->life,
b->extra->points, b->extra->peopleHeld);
}
// Score a collection of lines as a possible license plate region.
// If any segments are missing, extrapolate the missing pieces
void PlateCorners::scoreHorizontals(int h1, int h2)
{
//if (this->debug)
// cout << "PlateCorners::scorePlate" << endl;
float score = 0; // Lower is better
LineSegment top;
LineSegment bottom;
float charHeightToPlateHeightRatio = config->plateHeightMM / config->charHeightMM;
float idealPixelHeight = this->charHeight * charHeightToPlateHeightRatio;
if (h1 == NO_LINE && h2 == NO_LINE)
{
// return;
Point centerLeft = charRegion->getCharBoxLeft().midpoint();
Point centerRight = charRegion->getCharBoxRight().midpoint();
LineSegment centerLine = LineSegment(centerLeft.x, centerLeft.y, centerRight.x, centerRight.y);
top = centerLine.getParallelLine(idealPixelHeight / 2);
bottom = centerLine.getParallelLine(-1 * idealPixelHeight / 2 );
score += SCORING_MISSING_SEGMENT_PENALTY_HORIZONTAL * 2;
}
else if (h1 != NO_LINE && h2 != NO_LINE)
{
top = this->plateLines->horizontalLines[h1];
bottom = this->plateLines->horizontalLines[h2];
}
else if (h1 == NO_LINE && h2 != NO_LINE)
{
bottom = this->plateLines->horizontalLines[h2];
top = bottom.getParallelLine(idealPixelHeight);
score += SCORING_MISSING_SEGMENT_PENALTY_HORIZONTAL;
}
else if (h1 != NO_LINE && h2 == NO_LINE)
{
top = this->plateLines->horizontalLines[h1];
bottom = top.getParallelLine(-1 * idealPixelHeight);
score += SCORING_MISSING_SEGMENT_PENALTY_HORIZONTAL;
}
// Make sure this line is above our license plate letters
if (top.isPointBelowLine(charRegion->getCharBoxTop().midpoint()) == false)
return;
// Make sure this line is below our license plate letters
if (bottom.isPointBelowLine(charRegion->getCharBoxBottom().midpoint()))
return;
// We now have 4 possible lines. Let's put them to the test and score them...
/////////////////////////////////////////////////////////////////////////
// Score "Boxiness" of the 4 lines. How close is it to a parallelogram?
/////////////////////////////////////////////////////////////////////////
float horizontalAngleDiff = abs(top.angle - bottom.angle);
score += (horizontalAngleDiff) * SCORING_BOXINESS_WEIGHT;
// if (this->debug)
// cout << "PlateCorners boxiness score: " << (horizontalAngleDiff + verticalAngleDiff) * SCORING_BOXINESS_WEIGHT << endl;
//////////////////////////////////////////////////////////////////////////
// SCORE the shape wrt character position and height relative to position
//////////////////////////////////////////////////////////////////////////
Point topPoint = top.midpoint();
Point botPoint = bottom.closestPointOnSegmentTo(topPoint);
float plateHeightPx = distanceBetweenPoints(topPoint, botPoint);
// Get the height difference
float heightRatio = charHeight / plateHeightPx;
float idealHeightRatio = (config->charHeightMM / config->plateHeightMM);
//if (leftRatio < MIN_CHAR_HEIGHT_RATIO || leftRatio > MAX_CHAR_HEIGHT_RATIO || rightRatio < MIN_CHAR_HEIGHT_RATIO || rightRatio > MAX_CHAR_HEIGHT_RATIO)
float heightRatioDiff = abs(heightRatio - idealHeightRatio);
// Ideal ratio == ~.45
// Get the distance from the top and the distance from the bottom
// Take the average distances from the corners of the character region to the top/bottom lines
// float topDistance = distanceBetweenPoints(topMidLinePoint, charRegion->getCharBoxTop().midpoint());
// float bottomDistance = distanceBetweenPoints(bottomMidLinePoint, charRegion->getCharBoxBottom().midpoint());
// float idealTopDistance = charHeight * (TOP_WHITESPACE_HEIGHT_MM / CHARACTER_HEIGHT_MM);
// float idealBottomDistance = charHeight * (BOTTOM_WHITESPACE_HEIGHT_MM / CHARACTER_HEIGHT_MM);
// float distScore = abs(topDistance - idealTopDistance) + abs(bottomDistance - idealBottomDistance);
score += heightRatioDiff * SCORING_PLATEHEIGHT_WEIGHT;
//////////////////////////////////////////////////////////////////////////
// SCORE the middliness of the stuff. We want our top and bottom line to have the characters right towards the middle
//////////////////////////////////////////////////////////////////////////
Point charAreaMidPoint = charRegion->getCharBoxLeft().midpoint();
Point topLineSpot = top.closestPointOnSegmentTo(charAreaMidPoint);
Point botLineSpot = bottom.closestPointOnSegmentTo(charAreaMidPoint);
float topDistanceFromMiddle = distanceBetweenPoints(topLineSpot, charAreaMidPoint);
float bottomDistanceFromMiddle = distanceBetweenPoints(topLineSpot, charAreaMidPoint);
float idealDistanceFromMiddle = idealPixelHeight / 2;
float middleScore = abs(topDistanceFromMiddle - idealDistanceFromMiddle) + abs(bottomDistanceFromMiddle - idealDistanceFromMiddle);
score += middleScore * SCORING_TOP_BOTTOM_SPACE_VS_CHARHEIGHT_WEIGHT;
// if (this->debug)
// {
// cout << "PlateCorners boxiness score: " << avgRatio * SCORING_TOP_BOTTOM_SPACE_VS_CHARHEIGHT_WEIGHT << endl;
// cout << "PlateCorners boxiness score: " << distScore * SCORING_PLATEHEIGHT_WEIGHT << endl;
// }
//////////////////////////////////////////////////////////////
// SCORE: the shape for angles matching the character region
//////////////////////////////////////////////////////////////
float charanglediff = abs(charAngle - top.angle) + abs(charAngle - bottom.angle);
score += charanglediff * SCORING_ANGLE_MATCHES_LPCHARS_WEIGHT;
// if (this->debug)
// cout << "PlateCorners boxiness score: " << charanglediff * SCORING_ANGLE_MATCHES_LPCHARS_WEIGHT << endl;
if (score < this->bestHorizontalScore)
{
float scorecomponent;
if (this->config->debugPlateCorners)
{
cout << "xx xx Score: charHeight " << this->charHeight << endl;
cout << "xx xx Score: idealHeight " << idealPixelHeight << endl;
cout << "xx xx Score: h1,h2= " << h1 << "," << h2 << endl;
cout << "xx xx Score: Top= " << top.str() << endl;
cout << "xx xx Score: Bottom= " << bottom.str() << endl;
cout << "Horizontal breakdown Score:" << endl;
cout << " -- Boxiness Score: " << horizontalAngleDiff << " -- Weight (" << SCORING_BOXINESS_WEIGHT << ")" << endl;
scorecomponent = horizontalAngleDiff * SCORING_BOXINESS_WEIGHT;
cout << " -- -- Score: " << scorecomponent << " = " << scorecomponent / score * 100 << "% of score" << endl;
cout << " -- Height Ratio Diff Score: " << heightRatioDiff << " -- Weight (" << SCORING_PLATEHEIGHT_WEIGHT << ")" << endl;
scorecomponent = heightRatioDiff * SCORING_PLATEHEIGHT_WEIGHT;
cout << " -- -- " << scorecomponent << " = " << scorecomponent / score * 100 << "% of score" << endl;
cout << " -- Distance Score: " << middleScore << " -- Weight (" << SCORING_TOP_BOTTOM_SPACE_VS_CHARHEIGHT_WEIGHT << ")" << endl;
scorecomponent = middleScore * SCORING_TOP_BOTTOM_SPACE_VS_CHARHEIGHT_WEIGHT;
cout << " -- -- Score: " << scorecomponent << " = " << scorecomponent / score * 100 << "% of score" << endl;
cout << " -- Char angle Score: " << charanglediff << " -- Weight (" << SCORING_ANGLE_MATCHES_LPCHARS_WEIGHT << ")" << endl;
scorecomponent = charanglediff * SCORING_ANGLE_MATCHES_LPCHARS_WEIGHT;
cout << " -- -- Score: " << scorecomponent << " = " << scorecomponent / score * 100 << "% of score" << endl;
cout << " -- Score: " << score << endl;
}
this->bestHorizontalScore = score;
bestTop = LineSegment(top.p1.x, top.p1.y, top.p2.x, top.p2.y);
bestBottom = LineSegment(bottom.p1.x, bottom.p1.y, bottom.p2.x, bottom.p2.y);
}
}
void PlateCorners::scoreVerticals(int v1, int v2)
{
float score = 0; // Lower is better
LineSegment left;
LineSegment right;
float charHeightToPlateWidthRatio = config->plateWidthMM / config->charHeightMM;
float idealPixelWidth = this->charHeight * (charHeightToPlateWidthRatio * 1.03); // Add 3% so we don't clip any characters
if (v1 == NO_LINE && v2 == NO_LINE)
{
//return;
Point centerTop = charRegion->getCharBoxTop().midpoint();
Point centerBottom = charRegion->getCharBoxBottom().midpoint();
LineSegment centerLine = LineSegment(centerBottom.x, centerBottom.y, centerTop.x, centerTop.y);
left = centerLine.getParallelLine(idealPixelWidth / 2);
right = centerLine.getParallelLine(-1 * idealPixelWidth / 2 );
score += SCORING_MISSING_SEGMENT_PENALTY_VERTICAL * 2;
}
else if (v1 != NO_LINE && v2 != NO_LINE)
{
left = this->plateLines->verticalLines[v1];
right = this->plateLines->verticalLines[v2];
}
else if (v1 == NO_LINE && v2 != NO_LINE)
{
right = this->plateLines->verticalLines[v2];
left = right.getParallelLine(idealPixelWidth);
score += SCORING_MISSING_SEGMENT_PENALTY_VERTICAL;
}
else if (v1 != NO_LINE && v2 == NO_LINE)
{
left = this->plateLines->verticalLines[v1];
right = left.getParallelLine(-1 * idealPixelWidth);
score += SCORING_MISSING_SEGMENT_PENALTY_VERTICAL;
}
// Make sure this line is to the left of our license plate letters
if (left.isPointBelowLine(charRegion->getCharBoxLeft().midpoint()) == false)
return;
// Make sure this line is to the right of our license plate letters
if (right.isPointBelowLine(charRegion->getCharBoxRight().midpoint()))
return;
/////////////////////////////////////////////////////////////////////////
// Score "Distance from the edge...
/////////////////////////////////////////////////////////////////////////
float leftDistanceFromEdge = abs((float) (left.p1.x + left.p2.x) / 2);
float rightDistanceFromEdge = abs(this->inputImage.cols - ((float) (right.p1.x + right.p2.x) / 2));
float distanceFromEdge = leftDistanceFromEdge + rightDistanceFromEdge;
score += distanceFromEdge * SCORING_VERTICALDISTANCE_FROMEDGE_WEIGHT;
/////////////////////////////////////////////////////////////////////////
// Score "Boxiness" of the 4 lines. How close is it to a parallelogram?
/////////////////////////////////////////////////////////////////////////
float verticalAngleDiff = abs(left.angle - right.angle);
score += (verticalAngleDiff) * SCORING_BOXINESS_WEIGHT;
//////////////////////////////////////////////////////////////////////////
// SCORE the shape wrt character position and height relative to position
//////////////////////////////////////////////////////////////////////////
Point leftMidLinePoint = left.closestPointOnSegmentTo(charRegion->getCharBoxLeft().midpoint());
Point rightMidLinePoint = right.closestPointOnSegmentTo(charRegion->getCharBoxRight().midpoint());
float plateDistance = abs(idealPixelWidth - distanceBetweenPoints(leftMidLinePoint, rightMidLinePoint));
score += plateDistance * SCORING_VERTICALDISTANCE_WEIGHT;
if (score < this->bestVerticalScore)
{
float scorecomponent;
if (this->config->debugPlateCorners)
{
cout << "xx xx Score: charHeight " << this->charHeight << endl;
cout << "xx xx Score: idealwidth " << idealPixelWidth << endl;
cout << "xx xx Score: v1,v2= " << v1 << "," << v2 << endl;
cout << "xx xx Score: Left= " << left.str() << endl;
cout << "xx xx Score: Right= " << right.str() << endl;
cout << "Vertical breakdown Score:" << endl;
cout << " -- Boxiness Score: " << verticalAngleDiff << " -- Weight (" << SCORING_BOXINESS_WEIGHT << ")" << endl;
scorecomponent = verticalAngleDiff * SCORING_BOXINESS_WEIGHT;
cout << " -- -- Score: " << scorecomponent << " = " << scorecomponent / score * 100 << "% of score" << endl;
cout << " -- Distance From Edge Score: " << distanceFromEdge << " -- Weight (" << SCORING_VERTICALDISTANCE_FROMEDGE_WEIGHT << ")" << endl;
scorecomponent = distanceFromEdge * SCORING_VERTICALDISTANCE_FROMEDGE_WEIGHT;
cout << " -- -- Score: " << scorecomponent << " = " << scorecomponent / score * 100 << "% of score" << endl;
cout << " -- Distance Score: " << plateDistance << " -- Weight (" << SCORING_VERTICALDISTANCE_WEIGHT << ")" << endl;
scorecomponent = plateDistance * SCORING_VERTICALDISTANCE_WEIGHT;
cout << " -- -- Score: " << scorecomponent << " = " << scorecomponent / score * 100 << "% of score" << endl;
cout << " -- Score: " << score << endl;
}
this->bestVerticalScore = score;
bestLeft = LineSegment(left.p1.x, left.p1.y, left.p2.x, left.p2.y);
bestRight = LineSegment(right.p1.x, right.p1.y, right.p2.x, right.p2.y);
}
}
void CharacterAnalysis::filterBetweenLines(Mat img, TextContours& textContours, vector<TextLine> textLines )
{
static float MIN_AREA_PERCENT_WITHIN_LINES = 0.88;
static float MAX_DISTANCE_PERCENT_FROM_LINES = 0.15;
if (textLines.size() == 0)
return;
vector<Point> validPoints;
// Create a white mask for the area inside the polygon
Mat outerMask = Mat::zeros(img.size(), CV_8U);
for (unsigned int i = 0; i < textLines.size(); i++)
fillConvexPoly(outerMask, textLines[i].linePolygon.data(), textLines[i].linePolygon.size(), Scalar(255,255,255));
// For each contour, determine if enough of it is between the lines to qualify
for (unsigned int i = 0; i < textContours.size(); i++)
{
if (textContours.goodIndices[i] == false)
continue;
float percentInsideMask = getContourAreaPercentInsideMask(outerMask,
textContours.contours,
textContours.hierarchy,
(int) i);
if (percentInsideMask < MIN_AREA_PERCENT_WITHIN_LINES)
{
// Not enough area is inside the lines.
if (config->debugCharAnalysis)
cout << "Rejecting due to insufficient area" << endl;
textContours.goodIndices[i] = false;
continue;
}
// now check to make sure that the top and bottom of the contour are near enough to the lines
// First get the high and low point for the contour
// Remember that origin is top-left, so the top Y values are actually closer to 0.
Rect brect = boundingRect(textContours.contours[i]);
int xmiddle = brect.x + (brect.width / 2);
Point topMiddle = Point(xmiddle, brect.y);
Point botMiddle = Point(xmiddle, brect.y+brect.height);
// Get the absolute distance from the top and bottom lines
for (unsigned int i = 0; i < textLines.size(); i++)
{
Point closestTopPoint = textLines[i].topLine.closestPointOnSegmentTo(topMiddle);
Point closestBottomPoint = textLines[i].bottomLine.closestPointOnSegmentTo(botMiddle);
float absTopDistance = distanceBetweenPoints(closestTopPoint, topMiddle);
float absBottomDistance = distanceBetweenPoints(closestBottomPoint, botMiddle);
float maxDistance = textLines[i].lineHeight * MAX_DISTANCE_PERCENT_FROM_LINES;
if (absTopDistance < maxDistance && absBottomDistance < maxDistance)
{
// It's ok, leave it as-is.
}
else
{
textContours.goodIndices[i] = false;
if (config->debugCharAnalysis)
cout << "Rejecting due to top/bottom points that are out of range" << endl;
}
}
}
}
LinearAnimation::LinearAnimation(float span, vector<float> controlPoints): Animation(){
this->totalSpan = span * 1000;
this->controlPoints = controlPoints;
this->numTrajectories = (this->controlPoints.size() / 3) - 1;
this->totalDist = 0;
//Load trajectory distances, calculate total distance and coordinate deltas between trajectories
for(unsigned int i = 0; i < numTrajectories; i++) {
//Get this point and the next
float point1[3], point2[3];
int ind1 = (i * 3);
int ind2 = ((i+1) * 3);
point1[0] = controlPoints.at(0 + ind1);
point1[1] = controlPoints.at(1 + ind1);
point1[2] = controlPoints.at(2 + ind1);
point2[0] = controlPoints.at(0 + ind2);
point2[1] = controlPoints.at(1 + ind2);
point2[2] = controlPoints.at(2 + ind2);
//Calculate distance between those 2 points and add it to total distance and trajectory distances vector
float trajDist = distanceBetweenPoints(point1, point2);
totalDist += trajDist;
trajectoryDists.push_back(trajDist);
//Calculate deltas between coordinates of the two points and add them to deltas vector
float deltaX;
float deltaY;
float deltaZ;
deltaX = point2[0] - point1[0];
deltaY = point2[1] - point1[1];
deltaZ = point2[2] - point1[2];
trajectoryCoordDeltas.push_back(deltaX);
trajectoryCoordDeltas.push_back(deltaY);
trajectoryCoordDeltas.push_back(deltaZ);
//Calculate the previous offsets for the trajectory coordinates (that is, the sum of the previous deltas), and push them to the vector
int previousIndex;
if(i == 0) {
trajectoryCoordPreviousOffsets.push_back(0);
trajectoryCoordPreviousOffsets.push_back(0);
trajectoryCoordPreviousOffsets.push_back(0);
previousIndex = 0;
}
else
previousIndex = i*3;
if(i < (numTrajectories - 1) ) {
trajectoryCoordPreviousOffsets.push_back(deltaX + trajectoryCoordPreviousOffsets.at(previousIndex + 0));
trajectoryCoordPreviousOffsets.push_back(deltaY + trajectoryCoordPreviousOffsets.at(previousIndex + 1));
trajectoryCoordPreviousOffsets.push_back(deltaZ + trajectoryCoordPreviousOffsets.at(previousIndex + 2));
}
}
//Calculate angles between the trajectories based on the trajectory vector components on the XZ plane, always summing the angle with the previous one
for(unsigned int i = 0; i < numTrajectories - 1; i++) {
float previousAngle;
if(i == 0)
previousAngle = 0;
else
previousAngle = trajectoryAngles.at(i - 1);
int ind = i * 3;
int ind2 = i * 3 + 3;
float vector1[3];
vector1[0] = trajectoryCoordDeltas.at(ind + 0);
//Since we want the vector projections in the XZ plane, we consider y = 0
vector1[1] = 0;
vector1[2] = trajectoryCoordDeltas.at(ind + 2);
float vector2[3];
vector2[0] = trajectoryCoordDeltas.at(ind2 + 0);
//Since we want the vector projections in the XZ plane, we consider y = 0
vector2[1] = 0;
vector2[2] = trajectoryCoordDeltas.at(ind2 + 2);
float rotAngle = angleBetweenVectors(vector2, vector1);
//To ensure we use angles between 0 and 360, for simplicity
if(rotAngle > 360)
rotAngle -= 360;
else if(rotAngle < 0)
rotAngle += 360;
rotAngle += previousAngle;
trajectoryAngles.push_back(rotAngle);
}
// Using the trajectory distances and the total distance, calculate the fraction of total animation
//that each trajectory represents, and thus calculate the different time spans each trajectory
//shall need to complete its animation.
for(unsigned int i = 0; i < numTrajectories; i++) {
float distFraction = trajectoryDists.at(i) / totalDist;
unsigned long timeSpan = (distFraction * totalSpan);
timeSpans.push_back(timeSpan);
}
currentTrajectory = 0;
elapsedTimeInTraj = 0;
totalElapsedTime = 0;
ended = false;
}
static bool compareDistanceBetweenPoints(const Platform::IntPoint& p, const Platform::IntRectRegion& r1, const Platform::IntRectRegion& r2)
{
return distanceBetweenPoints(p, r1.extents().center()) > distanceBetweenPoints(p, r2.extents().center());
}
TEST(PointTest, distanceBetweenPointsTest) {
Point p1{-0.35312, 341.1231251134}, p2{345345.242, -323423.23};
EXPECT_DOUBLE_EQ(473378.21709845297, distanceBetweenPoints(p1, p2));
}
/**
* Updates path logic.
*/
void PathHandler::updatePath()
{
geometry_msgs::Twist command;
publishState();
// no path? nothing to handle
if (getPathSize() == 0)
return;
// update our position if possible.
if (updateCurrentPosition() == false)
{
ROS_ERROR("Failed to update robot position.");
return;
}
if (mCurrentPathIndex < getPathSize() - 1) // we are moving along a line segment in the path
{
geometry_msgs::Point robotPos;
robotPos.x = mRobotPosition.getOrigin().x();
robotPos.y = mRobotPosition.getOrigin().y();
geometry_msgs::Point closestOnPath = closestPointOnLine(robotPos, mPath[mCurrentPathIndex].position, mPath[mCurrentPathIndex + 1].position);
//ROS_INFO("robotPos[%lf, %lf], closestOnPath[%lf, %lf]", robotPos.x, robotPos.y, closestOnPath.x, closestOnPath.y);
if (distanceBetweenPoints(closestOnPath, robotPos) > mResetDistanceTolerance)
{
ROS_INFO("Drove too far away from path, re-sending goal.");
mFollowState = FOLLOW_STATE_IDLE;
resendGoal();
clearPath();
return;
}
geometry_msgs::Point pointOnPath = getPointOnPathWithDist(closestOnPath, mLookForwardDistance);
//ROS_INFO("closestOnPath[%lf, %lf], pointOnPath[%lf, %lf]", closestOnPath.x, closestOnPath.y, pointOnPath.x, pointOnPath.y);
double angle = angleTo(pointOnPath);
/*double robotYaw = tf::getYaw(mRobotPosition.getRotation()); // only used for printing
ROS_INFO("Follow state: %d Robot pos: (%lf, %lf, %lf). Target pos: (%lf, %lf, %lf). RobotYaw: %lf. FocusYaw: %lf", mFollowState, mRobotPosition.getOrigin().getX(),
mRobotPosition.getOrigin().getY(), mRobotPosition.getOrigin().getZ(), pointOnPath.x,
pointOnPath.y, pointOnPath.z, robotYaw, angle);*/
//ROS_INFO("Angle to path: %f", angle);
command.linear.x = getScaledLinearSpeed(angle);
command.angular.z = getScaledAngularSpeed(angle);
if (distanceBetweenPoints(closestOnPath, mPath[mCurrentPathIndex + 1].position) < mDistanceTolerance)
{
//ROS_INFO("Moving to next line segment on path.");
mCurrentPathIndex++;
}
publishLocalPath(command.linear.x * 3, angle);
}
else // only rotate for final yaw
{
double yaw = tf::getYaw(mPath[getPathSize() - 1].orientation);
btVector3 orientation = btVector3(cos(yaw), sin(yaw), 0.0).rotate(btVector3(0,0,1), -tf::getYaw(mRobotPosition.getRotation()));
double angle = atan2(orientation.getY(), orientation.getX());
//ROS_INFO("Angle to final orientation: %f", angle);
if (fabs(angle) > mFinalYawTolerance)
command.angular.z = getScaledAngularSpeed(angle, true);
else
{
// path is done!
mFollowState = FOLLOW_STATE_FINISHED;
clearPath();
}
publishLocalPath(1.0, angle);
}
mCommandPub.publish(command);
}
// 0.000011479429428388439 gps points per meter
void waypointManager(void){
waypoint * newWP = (waypoint *) malloc(sizeof(waypoint));
dataGPS curPosGPS;
//int gpsRet;
float turnHeading;
double tolerance = 3; // tolerance in meters
printf("Beginning\r\n");
addWaypointxy(-87.322314, 39.483694);
newWP = getCurrentWaypoint();
printf("\r\nTrying to initialize GPS");
//gpsRet = init_GPS();
curPosGPS = getGPS(gpsRet);
if(!curPosGPS.valid){
for(int i = 0; i < 10 && !curPosGPS.valid; i++){
sleep(5);
printf("\r\nWaiting for valid GPS data....\r\n");
curPosGPS = getGPS(gpsRet);
}
}
waypoint * curPosWP = (waypoint *) malloc(sizeof(waypoint));
curPosWP->x = curPosGPS.x;
curPosWP->y = curPosGPS.y;
turnHeading = angleBetweenPoints(*newWP, *curPosWP);
turn(turnHeading);
// at this point we should be pointing in the right direction
double line_mb[2];
findLine(*curPosWP, * newWP, line_mb);
printf("\r\nCurrent position: x %lf, y %lf\r\n", curPosWP->x, curPosWP->y);
printf("Waypoint to move to: x %lf, y %lf\r\n", newWP->x, newWP->y);
printf("Turn heading: %f\r\n", turnHeading);
printf("Line: m %lf, b %lf\r\n", line_mb[0], line_mb[1]);
int waypointsLeft = 1;
while(keepGoing && waypointsLeft){
while(keepGoing && distanceBetweenPoints(*curPosWP, *newWP) > tolerance*0.000011479429428388439){
// go straight
printf("Going straight\r\ndistance to target: %lf\r\ntolerance: %lf\r\n", distanceBetweenPoints(*curPosWP, *newWP), tolerance*0.000011479429428388439);
gpio_set_value(LEFT_FWD_GPIO, 1);
gpio_set_value(LEFT_BCK_GPIO, 0);
gpio_set_value(RIGHT_FWD_GPIO, 1);
gpio_set_value(RIGHT_BCK_GPIO, 0);
curPosGPS = getGPS(gpsRet);
curPosWP->x = curPosGPS.x;
curPosWP->y = curPosGPS.y;
if(!bbCheck(line_mb[0], line_mb[1], * curPosWP, tolerance)){
returnToPreviousWaypoint();
printf("We're outside of the bounds. Returning to the previous waypoint\r\n");
break; // out of the bounding box
}
sleep(2);
}
printf("We've hit the waypoint! Stopping the motors...\r\n");
gpio_set_value(LEFT_FWD_GPIO, 0);
gpio_set_value(LEFT_BCK_GPIO, 0);
gpio_set_value(RIGHT_FWD_GPIO, 0);
gpio_set_value(RIGHT_BCK_GPIO, 0);
waypointsLeft = (advanceToNextWaypoint() != -1); // don't keep going if we've reached the end of the waypoints
newWP = getCurrentWaypoint(); // if we've already advanced past the end this will give us the last WP, but
// we will break out of this loop anyway
printf("KeepGoing: %d...should be 0 if we've hit the end of the waypoints", keepGoing);
}
free(curPosWP);
free(newWP);
}
vector<bool> CharacterAnalysis::filterBetweenLines(Mat img, vector<vector<Point> > contours, vector<Vec4i> hierarchy, vector<Point> outerPolygon, vector<bool> goodIndices)
{
static float MIN_AREA_PERCENT_WITHIN_LINES = 0.88;
vector<bool> includedIndices(contours.size());
for (int j = 0; j < contours.size(); j++)
includedIndices[j] = false;
if (outerPolygon.size() == 0)
return includedIndices;
vector<Point> validPoints;
// Figure out the line height
LineSegment topLine(outerPolygon[0].x, outerPolygon[0].y, outerPolygon[1].x, outerPolygon[1].y);
LineSegment bottomLine(outerPolygon[3].x, outerPolygon[3].y, outerPolygon[2].x, outerPolygon[2].y);
float x = ((float) img.cols) / 2;
Point midpoint = Point(x, bottomLine.getPointAt(x));
Point acrossFromMidpoint = topLine.closestPointOnSegmentTo(midpoint);
float lineHeight = distanceBetweenPoints(midpoint, acrossFromMidpoint);
// Create a white mask for the area inside the polygon
Mat outerMask = Mat::zeros(img.size(), CV_8U);
Mat innerArea = Mat::zeros(img.size(), CV_8U);
fillConvexPoly(outerMask, outerPolygon.data(), outerPolygon.size(), Scalar(255,255,255));
for (int i = 0; i < contours.size(); i++)
{
if (goodIndices[i] == false)
continue;
// get rid of the outline by drawing a 1 pixel width black line
drawContours(innerArea, contours,
i, // draw this contour
cv::Scalar(255,255,255), // in
CV_FILLED,
8,
hierarchy,
0
);
bitwise_and(innerArea, outerMask, innerArea);
vector<vector<Point> > tempContours;
findContours(innerArea, tempContours,
CV_RETR_EXTERNAL, // retrieve the external contours
CV_CHAIN_APPROX_SIMPLE ); // all pixels of each contours );
double totalArea = contourArea(contours[i]);
double areaBetweenLines = 0;
for (int tempContourIdx = 0; tempContourIdx < tempContours.size(); tempContourIdx++)
{
areaBetweenLines += contourArea(tempContours[tempContourIdx]);
}
if (areaBetweenLines / totalArea >= MIN_AREA_PERCENT_WITHIN_LINES)
{
includedIndices[i] = true;
}
innerArea.setTo(Scalar(0,0,0));
}
return includedIndices;
}
