tLane LaneDetector::DetectLanes(Mat &inputImage, Logger &logger)
{
DebugImage = Mat::zeros(200, 300, CV_8UC3);
Mat binaryImage = MakeBinary(inputImage);
// Mat kernel = getStructuringElement(CV_SHAPE_ELLIPSE, Size(5, 5));
// morphologyEx(workingImage, workingImage, MORPH_CLOSE, kernel);
if (reseted)
{
ExtractBorder(inputImage);
CrossingDetector leftBot("left_bot.bmp", Point(28, 24), &IsLeft, 0.6);
CrossingDetector leftTop("left_top.bmp", Point(25, 39), &IsLeft, 0.55);
BaseLaneLine *left = new InitLine(0, cvRound(binaryImage.cols / 2.0) - 20, border, CLOCKWISE, leftBot, leftTop, laneLogger);
CrossingDetector rightBot("right_bot.bmp", Point(13, 12), &IsRight, 0.55);
CrossingDetector rightTop("right_top.bmp", Point(9, 25), &IsRight, 0.55);
BaseLaneLine *right = new InitLine(cvRound(binaryImage.cols / 2.0) - 20 + 1, binaryImage.cols - 1, border,
COUNTER_CLOCKWISE, rightBot, rightTop, laneLogger);
leftLine = Ptr<BaseLaneLine>(left);
rightLine = Ptr<BaseLaneLine>(right);
reseted = false;
tLane result = GenerateNormalizedLaneStruct(binaryImage);
return result;
}
leftLine->Log("Left:");
leftLine = leftLine->UpdateLaneLine(binaryImage, *rightLine);
leftLine->Log("------");
rightLine->Log("Right:");
rightLine = rightLine->UpdateLaneLine(binaryImage, *leftLine);
rightLine->Log("------");
#ifndef NDEBUG
cvtColor(binaryImage, DebugImage, CV_GRAY2RGB);
leftLine->Draw(DebugImage);
rightLine->Draw(DebugImage);
line(DebugImage, Point2f(DebugImage.cols / 2.0f - (15 + 8 + 47), 0), Point2f(DebugImage.cols / 2.0f - (15 + 8 + 47), 199), CV_RGB(0, 0, 255));
line(DebugImage, Point2f(DebugImage.cols / 2.0f + (15 + 8), 0), Point2f(DebugImage.cols / 2.0f + (15 + 8), 199), CV_RGB(0, 0, 255));
#endif
tLane result = GenerateNormalizedLaneStruct(binaryImage);
return result;
}
bool NRectangle::draw(NGenericPixelMatrix *matrix, NPixelBox geometry, DrawOpt op) {
if(!pixelBoxInLcd(geometry)) return false;
NPoint rightTop(geometry.base().x() + geometry.width(), geometry.base().y());
NPoint rightBottom(geometry.base().x() + geometry.width(), geometry.base().y() + geometry.height());
NPoint leftBottom(geometry.base().x(), geometry.base().y() + geometry.height());
NLine::draw(matrix, geometry.base(), rightTop, op);
NLine::draw(matrix, rightTop.setY(rightTop.y() + 1), rightBottom, op);
NLine::draw(matrix, rightBottom.setX(rightBottom.x() - 1), leftBottom, op);
NLine::draw(matrix, leftBottom.setY(leftBottom.y()-1), geometry.base().setY(geometry.base().y()+1), op);
return true;
}
void Draw::DrawBox(const fRect &rect, float rotation, const cColorRGBA &color, Texture *texture, Shader *shader)
{
VertexDraw2D *buffer = LE_DrawManager.GetVertexBuffer2D(6);
fSize screenSize(LE_DrawManager.GetVirtualScreenSize());
fRect rectInProj = rect / screenSize;
fPoint rightTop(rect.width, 0.0f);
fPoint rightBottom(rect.width, rect.height);
fPoint leftBottom(0.0f, rect.height);
fMatrix4x4 rotmat = fMatrix4x4::RotationZ(rotation);
rightBottom = rotmat * rightBottom / screenSize;
rightBottom += rectInProj.xy();
rightTop = rotmat * rightTop / screenSize;
rightTop += rectInProj.xy();
leftBottom = rotmat * leftBottom / screenSize;
leftBottom += rectInProj.xy();
buffer[0].SetPosition(fVector3(rectInProj.xy(), 0.0f));
buffer[0].SetColor(color);
buffer[0].SetTexcoord(fVector2(0.0f, 0.0f));
buffer[1].SetPosition(fVector3(leftBottom, 0.0f));
buffer[1].SetColor(color);
buffer[1].SetTexcoord(fVector2(0.0f, 1.0f));
buffer[2].SetPosition(fVector3(rightTop, 0.0f));
buffer[2].SetColor(color);
buffer[2].SetTexcoord(fVector2(1.0f, 0.0f));
buffer[3].SetPosition(fVector3(leftBottom, 0.0f));
buffer[3].SetColor(color);
buffer[3].SetTexcoord(fVector2(0.0f, 1.0f));
buffer[4].SetPosition(fVector3(rightBottom, 0.0f));
buffer[4].SetColor(color);
buffer[4].SetTexcoord(fVector2(1.0f, 1.0f));
buffer[5].SetPosition(fVector3(rightTop, 0.0f));
buffer[5].SetColor(color);
buffer[5].SetTexcoord(fVector2(1.0f, 0.0f));
if(shader == NULL) {
if(texture) {
DrawCommand::BindTexture(0, texture);
} else {
DrawCommand::BindTexture(0, RenderContext::GetSingleton().GetDefaultTexture(RenderContext::DefaultTextureTypeWhite));
}
}
LE_DrawManager.Draw2DFlush(GraphicsFlag::PT_TRIANGLELIST, shader);
}
void UiEditorPanel::drawFrame()
{
CCSize size = CCSize(1000, 800);
CCPoint leftBottom(0, 0);
CCPoint leftTop(0, size.height);
CCPoint rightBottom(size.width, 0);
CCPoint rightTop(size.width, size.height);
CCDrawNode *drawNode = CCDrawNode::create();
addChild(drawNode);
drawNode->drawSegment(leftBottom, rightBottom, 0.5f, ccc4f(0.5f, 0.5f, 0.5f, 1));
drawNode->drawSegment(rightBottom, rightTop, 0.5f, ccc4f(0.5f, 0.5f, 0.5f, 1));
drawNode->drawSegment(rightTop, leftTop, 0.5f, ccc4f(0.5f, 0.5f, 0.5f, 1));
drawNode->drawSegment(leftTop, leftBottom, 0.5f, ccc4f(0.5f, 0.5f, 0.5f, 1));
auto winSize = CCDirector::sharedDirector()->getWinSize();
auto offset = ccp((winSize.width - size.width) * 0.5f, (winSize.height - size.height) * 0.5f);
drawNode->setPosition(offset);
}
void Sprite::Draw(const iRect &frame, const iRect &rect, float rotation)
{
if (_texture) DrawCommand::BindTexture(0, _texture);
fSize texSize(_texture->GetSize());
fSize screenSize(LE_DrawManager.GetVirtualScreenSize());
VertexDraw2D *buffer = LE_DrawManager.GetVertexBuffer2D(6);
fRect frameInProj = frame / texSize;
fRect rectInProj = rect / screenSize;
fPoint rightTop(float(rect.width), 0.0f);
fPoint rightBottom(float(rect.width), float(rect.height));
fPoint leftBottom(0.0f, float(rect.height));
fMatrix4x4 rotmat = fMatrix4x4::RotationZ(rotation);
rightBottom = rotmat * rightBottom / screenSize;
rightBottom += rectInProj.xy();
rightTop = rotmat * rightTop / screenSize;
rightTop += rectInProj.xy();
leftBottom = rotmat * leftBottom / screenSize;
leftBottom += rectInProj.xy();
buffer[0].SetPosition(rectInProj.xy());
buffer[0].SetColor(0xffffffff);
buffer[0].SetTexcoord(fVector2(frameInProj.x, frameInProj.y));
buffer[1].SetPosition(leftBottom);
buffer[1].SetColor(0xffffffff);
buffer[1].SetTexcoord(fVector2(frameInProj.x, frameInProj.y+frameInProj.height));
buffer[2].SetPosition(rightTop);
buffer[2].SetColor(0xffffffff);
buffer[2].SetTexcoord(fVector2(frameInProj.x + frameInProj.width, frameInProj.y));
buffer[3].SetPosition(leftBottom);
buffer[3].SetColor(0xffffffff);
buffer[3].SetTexcoord(fVector2(frameInProj.x, frameInProj.y + frameInProj.height));
buffer[4].SetPosition(rightBottom);
buffer[4].SetColor(0xffffffff);
buffer[4].SetTexcoord(fVector2(frameInProj.x + frameInProj.width, frameInProj.y + frameInProj.height));
buffer[5].SetPosition(rightTop);
buffer[5].SetColor(0xffffffff);
buffer[5].SetTexcoord(fVector2(frameInProj.x + frameInProj.width, frameInProj.y));
LE_DrawManager.Draw2DFlush(GraphicsFlag::PT_TRIANGLELIST);
}
void
Draw::DrawBox(const fRect &rect, float rotation, const cColorRGBA &leftTopColor, const cColorRGBA &rightTopColor, const cColorRGBA &leftBottomColor, const cColorRGBA &rightBottomColor)
{
VertexDraw2D *buffer = LE_DrawManager.GetVertexBuffer2D(6);
fSize screenSize(LE_DrawManager.GetVirtualScreenSize());
fRect rectInProj = rect / screenSize;
fPoint rightTop(rect.width, 0.0f);
fPoint rightBottom(rect.width, rect.height);
fPoint leftBottom(0.0f, rect.height);
fMatrix4x4 rotmat = fMatrix4x4::RotationZ(rotation);
rightBottom = rotmat * rightBottom / screenSize;
rightBottom += rectInProj.xy();
rightTop = rotmat * rightTop / screenSize;
rightTop += rectInProj.xy();
leftBottom = rotmat * leftBottom / screenSize;
leftBottom += rectInProj.xy();
buffer[0].SetPosition(fVector3(rectInProj.xy(), 0.0f));
buffer[0].SetColor(leftTopColor);
buffer[0].SetTexcoord(fVector2(0.0f, 0.0f));
buffer[1].SetPosition(fVector3(leftBottom, 0.0f));
buffer[1].SetColor(leftBottomColor);
buffer[1].SetTexcoord(fVector2(0.0f, 1.0f));
buffer[2].SetPosition(fVector3(rightTop, 0.0f));
buffer[2].SetColor(rightTopColor);
buffer[2].SetTexcoord(fVector2(1.0f, 0.0f));
buffer[3].SetPosition(fVector3(leftBottom, 0.0f));
buffer[3].SetColor(leftBottomColor);
buffer[3].SetTexcoord(fVector2(0.0f, 1.0f));
buffer[4].SetPosition(fVector3(rightBottom, 0.0f));
buffer[4].SetColor(rightBottomColor);
buffer[4].SetTexcoord(fVector2(1.0f, 1.0f));
buffer[5].SetPosition(fVector3(rightTop, 0.0f));
buffer[5].SetColor(rightTopColor);
buffer[5].SetTexcoord(fVector2(1.0f, 0.0f));
DrawCommand::BindTexture(0, 0);
LE_DrawManager.Draw2DFlush(GraphicsFlag::PT_TRIANGLELIST);
}
void LivingRoom::draw(Renderer *renderer, const Mat4 &transform, uint32_t flags)
{
//画线，用于测试像素坐标和格子坐标的转换
Vec2 point = Vec2(GRIDMAP_ORIGIN_VERTEX_X, GRIDMAP_ORIGIN_VERTEX_Y);
float slopeRightTop = 1.0 / std::sqrt(3.0);
float slopeLeftTop = -1.0 / std::sqrt(3.0);
int unitX = GRID_WIDTH_X / 2;
int unitY = GRID_WIDTH_Y / 2;
glLineWidth(1.0f);
for (int i = 0; i < 19; ++i)
{
Vec2 leftBottom(0, (0 - point.x) * slopeRightTop + point.y);
Vec2 rightTop(1080, (1080 - point.x) * slopeRightTop + point.y);
DrawPrimitives::drawLine(leftBottom, rightTop);
Vec2 leftTop(0, (0 - point.x) * slopeLeftTop + point.y);
Vec2 rightBottom(1080, (1080 - point.x) * slopeLeftTop + point.y);
DrawPrimitives::drawLine(leftTop, rightBottom);
point.y -= unitY * 2;
}
}
Point3D BodyRegion::getPosition(Region region)
{
Point3D ret = m_center;
switch (region)
{
case LeftTop: ret = leftTop(); break;
case Top: ret = top(); break;
case RightTop: ret = rightTop(); break;
case Left: ret = left(); break;
case Center: ret = center(); break;
case Right: ret = right(); break;
case LeftBottom: ret = leftBottom(); break;
case Bottom: ret = bottom(); break;
case RightBottom: ret = rightBottom(); break;
default:
break;
}
return ret;
}
void NaoHeadControl::ScanAwayFromBall()
{
float neckYaw, headPitch;
if ( headPathPlanner.isLastPathFinished())
{
if (lastScanWasLeft)
{
Vector3f centerTop(0.0,45.0,headPathPlanner.lastNeckYaw), rightTop(0.0,45.0,-120.0), rightBottom(0.0,-45.0,-120.0);
Vector3f points[4]={centerTop,rightTop, rightBottom, headDown};
headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3f), 600);
}
else
{
Vector3f centerTop(0.0,45.0,headPathPlanner.lastNeckYaw), leftTop(0.0,45.0,120.0), leftBottom(0.0,-45.0,120.0);
Vector3f points[4]={centerTop,leftTop, leftBottom, headDown};
headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3f), 600);
}
lastScanWasLeft = !lastScanWasLeft;
}
headPathPlanner.getAngles(neckYaw, headPitch);
setJointsDirect(neckYaw, headPitch);
}
void GraphicsButton::setup() {
QRectF r = this->boundingRect();
int mx = m_marginLeft;
/// TODO get from keyboard definition
int boxWidth = r.width() - m_marginLeft - m_marginRight;
int boxHeight = r.height() - m_marginTop - m_marginBottom;
/**
* if group count = 2, then we have two columns,one aligned left, the other right
* if group count = 1, and level count = 2,we have one column, centrally aligned
* if group count = 1, and level count = 3,we have two column, left,aligned and right aligned
*
* Within each column, if one item, align central
* if two items, align higher level top, lower level bottom
*
1. one cell
2. two cells split vertically, left cell group1, right cell group 2
3. two cells split horizontally, bottom cell level1, top cell level2 (from group1)
4 three cells, split vertically, left cell split horizontally
left lower, group 1 level1
left top , group 1 level2
right group2 whatever level
5 four cells , both split horizontally and vertically
left low, group 1,level 1
left top, group 1,level 2
right low, group 2, level 1
right top, group 2, level 2
*/
// KeyDef * key = m_keydef;
switch(m_keydef->decorationCount()) {
case 1 : {
QRectF rect(mx,m_marginTop,boxWidth,boxHeight);
decorateKey(rect,1,1);
break;
}
case 2 : {
if (m_keydef->groupCount() == 1) { // split horizontally
qreal y = boxHeight/2;
QRectF top(mx,m_marginTop,boxWidth,y);
QRectF bottom(mx,m_marginTop + y, boxWidth,y);
decorateKey(bottom,1,1);
decorateKey(top,1,2);
}
else {
qreal x = boxWidth/2;
QRectF left(mx,m_marginTop,x,boxHeight);
QRectF right(x,m_marginTop,x,boxHeight);
decorateKey(left,1,1);
decorateKey(right,2,1);
}
break;
}
case 3 : {
qreal y = boxHeight/2;
qreal x = boxWidth/2;
QRectF leftTop(mx,m_marginTop,x,y);
QRectF leftBottom(mx,m_marginTop+y,x,y);
decorateKey(leftBottom,1,1);
decorateKey(leftTop,1,2);
/// if one group, but third level in the bottom right
if (m_keydef->groupCount() == 1) {
QRectF right(mx+x,m_marginTop+y,x,y);
decorateKey(right,1,3);
}
else { // otherwise put it in the middle
QRectF right(mx+x,m_marginTop,x,boxHeight);
decorateKey(right,2,1);
}
break;
}
case 4: {
qreal y = boxHeight/2;
qreal x = boxWidth/2;
QRectF leftTop(mx,m_marginTop,x,y);
QRectF leftBottom(mx,m_marginTop+y,x,y);
QRectF rightTop(mx+x,m_marginTop,x,y);
QRectF rightBottom(mx+x,m_marginTop+y,x,y);
decorateKey(leftBottom,1,1);
decorateKey(leftTop,1,2);
decorateKey(rightBottom,2,1);
decorateKey(rightTop,2,2);
break;
}
}
setPen(QPen(m_keyboardColor));
}
