/**
Draw the robot position in the original image sequence.
Need to know the entire camera calibration (and more) for this...
**/
QImage showRobotTrackUndistorted( IplImage* img, ///< original avi image
const RobotTracker* tracker, ///< robot tracker
int flip ///< flip output coords?
)
{
// Convert from IplImage to QImage...
const QSize imgSize( img->width,img->height );
QImage qimage = QImage( imgSize, QImage::Format_RGB888 );
CvMat mtxWrapper;
cvInitMatHeader( &mtxWrapper,
img->height,
img->width,
CV_8UC3,
qimage.bits() );
cvConvertImage( img, &mtxWrapper, flip );
// Now draw on top of the QImage...
QPainter painter(&qimage);
painter.setRenderHint(QPainter::Antialiasing, true);
QPen penRed( QColor(255,0,0) );
QPen penBlue( QColor(0,0,255) );
QPen penRedGreen( QColor(255,255,0) );
CvPoint2D32f offset = *tracker->GetOffsetParams();
const CameraCalibration* cal = tracker->GetCalibration();
// Draw the path taken so far (but each point needs to be converted from plane, to image co-ordinates)
const TrackHistory& history = tracker->GetHistory();
if ( history.size() > 1 )
{
CvPoint2D32f pos = history[0].GetPosition();
pos.x += offset.x;
pos.y += offset.y;
CvPoint2D32f oldPosf = cal->PlaneToImage( pos );
if ( flip )
{
oldPosf.y = img->height - oldPosf.y;
}
CvPoint oldPos = cvPoint( ( int )oldPosf.x, ( int )oldPosf.y );
for ( unsigned int i = 1; i < history.size(); ++i )
{
double tdiff = fabs( history[i].t() - history[i - 1].t() );
if ( tdiff > 2000.0 / 3.0 )
{
pos = history[i].GetPosition();
pos.x += offset.x;
pos.y += offset.y;
oldPosf = cal->PlaneToImage( pos );
if ( flip )
{
oldPosf.y = img->height - oldPosf.y;
}
oldPos = cvPoint( ( int )oldPosf.x, ( int )oldPosf.y );
}
else
{
pos = history[i].GetPosition();
pos.x += offset.x;
pos.y += offset.y;
CvPoint2D32f newPosf = cal->PlaneToImage( pos );
if ( flip )
{
newPosf.y = img->height - newPosf.y;
}
CvPoint newPos = cvPoint( ( int )newPosf.x, ( int )newPosf.y );
//CvScalar colour;
//float err = expf( -powf( (1.f-history[i].GetError())/0.5f, 4) );
//float col = 255*err;
//colour = CV_RGB( 255,0,0 ); //cvScalar( col, col, 255 );
// if ( err > 0.35f )
// {
// colour = cvScalar(0,255,0);
// }
// else
// {
// colour = cvScalar(0,0,255);
// }
//cvLine( img, oldPos, newPos, colour, 1, CV_AA );
penRed.setWidth( 2 );
painter.setPen( penRed );
painter.drawLine( oldPos.x, oldPos.y, newPos.x, newPos.y );
oldPos = newPos;
}
}
}
return qimage;
}
//画地图
void CRadarMapCtrl::DrawMap()
{
if(!m_map_bkgrdMap.bReady)
return;
CRect rect;
GetClientRect(&rect);
CDC * pDC=GetDC();
//得到画布
CDC memDC;
memDC.CreateCompatibleDC(pDC);
CBitmap bmp;
bmp.CreateCompatibleBitmap(pDC,rect.Width(),rect.Height());
CBitmap* oldbitmap=(CBitmap*)memDC.SelectObject(bmp);
//先把m_dcmemory的拷贝过来
memDC.BitBlt(0,0,rect.Width(),rect.Height(),&m_dcMemory,0,0,SRCCOPY);
int cnt=(m_map_bkgrdMap.stopAngle-m_map_bkgrdMap.startAngle)/m_map_angleDelta + 1;
CPoint points[1440];
points[0].x=m_width/2;
points[0].y=m_height/2;
//内部解析角度
double m_angleResolution=((double)(m_map_bkgrdMap.stopAngle-m_map_bkgrdMap.startAngle))/(m_map_bkgrdMap.mapDataLength-1);
int j=1;
for(int i=0;i<m_map_bkgrdMap.mapDataLength;i+=m_map_angleDelta)
{
int showPx=(int)(m_map_bkgrdMap.data[i]*m_map_scale/1000);
if(showPx>m_map_totalPx)
{
points[j]=ToCpoint((int)(m_map_bkgrdMap.startAngle+i*m_angleResolution),(int)((double)(m_map_totalPx/m_map_scale) * 1000));
}
else
{
points[j]=ToCpoint((int)(m_map_bkgrdMap.startAngle+i*m_angleResolution),m_map_bkgrdMap.data[i]);
}
//DrawPoint(i,m_map_bkgrdMap.data[i]);
//DrawLine(points[0],points[j]);
j++;
}
CPen penBlue(PS_SOLID, 1, RGB(50,200,255)); //蓝色
CPen* pOldPen = memDC.SelectObject(&penBlue);
CBrush brushRed(RGB(0, 50, 200)); //蓝色
CBrush* pOldBrush = memDC.SelectObject(&brushRed);
memDC.Polygon(points,j);
memDC.SelectObject(pOldPen);
memDC.SelectObject(pOldBrush);
///////////////半透明
BLENDFUNCTION bm;
bm.BlendOp=AC_SRC_OVER;
bm.BlendFlags=0;
bm.SourceConstantAlpha=230;//0--255, 0是完全透明
bm.AlphaFormat=0;
AlphaBlend(m_dcMemory.m_hDC,0,0,rect.Width(),rect.Height(),memDC.m_hDC,0,0,rect.Width(),rect.Height(),bm);
////////////////////////////////////////////////////////以下是正常画图
/*
if(!m_map_bkgrdMap.bReady)
return;
int cnt=(m_map_bkgrdMap.stopAngle-m_map_bkgrdMap.startAngle)/m_map_angleDelta + 1;
CPoint points[1440];
points[0].x=m_width/2;
points[0].y=m_height/2;
//内部解析角度
double m_angleResolution=((double)(m_map_bkgrdMap.stopAngle-m_map_bkgrdMap.startAngle))/(m_map_bkgrdMap.mapDataLength-1);
int j=1;
for(int i=0;i<m_map_bkgrdMap.mapDataLength;i+=m_map_angleDelta)
{
int showPx=(int)(m_map_bkgrdMap.data[i]*m_map_scale/1000);
if(showPx>m_map_totalPx)
{
points[j]=ToCpoint((int)(m_map_bkgrdMap.startAngle+i*m_angleResolution),(int)((double)(m_map_totalPx/m_map_scale) * 1000));
}
else
{
points[j]=ToCpoint((int)(m_map_bkgrdMap.startAngle+i*m_angleResolution),m_map_bkgrdMap.data[i]);
}
//DrawPoint(i,m_map_bkgrdMap.data[i]);
//DrawLine(points[0],points[j]);
j++;
}
//CPen penBlue(PS_SOLID, 1, RGB(255,0,0)); //默认
//CPen penBlue(PS_SOLID, 1, RGB(100,255,0)); //绿色
CPen penBlue(PS_SOLID, 1, RGB(50,200,255)); //蓝色
CPen* pOldPen = m_dcMemory.SelectObject(&penBlue);
//CBrush brushRed(RGB(123, 122, 23)); //默认
//CBrush brushRed(RGB(60, 200, 0)); //绿色
CBrush brushRed(RGB(0, 50, 200)); //蓝色
CBrush* pOldBrush = m_dcMemory.SelectObject(&brushRed);
m_dcMemory.Polygon(points,j);
//m_dcMemory.SelectObject(pOldPen);
//m_dcMemory.SelectObject(pOldBrush);
*/
}
