/** 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); */ }