SLAM - 利用Qt绘制灰度地图

SLAM - 利用Qt绘制灰度地图

MapWidget::MapWidget(QWidget *parent)    : QWidget(parent){    resize(WIDTH,HEIGHT);    scale_value = 1;    index = 0;    Zoom = 10;  // 缩放因子    imgWidth = this->width();    imgHeight = this->height();    for(int i=0;i<WIDTH;++i)        for(int j=0;j<HEIGHT;++j)            mapData.mapsum[i][j] = 255;    desImage = QImage(imgWidth,imgHeight,QImage::Format_RGB32);  // RGB32    // 初始化地图为白色    for(int i=0;i<imgWidth;++i)        for(int j=0;j<imgHeight;++j)            desImage.setPixel(i,j,qRgb(255,255,255));    timer = new QTimer(this);    connect(timer,SIGNAL(timeout()),this,SLOT(update()));    // 启动定时器    timer->start(100);}void MapWidget::paintEvent(QPaintEvent *){    QPainter painter(this);    if(index < TOTAL_LINES)    {        double x,y,theta,rho;        double tmpX = 0.0, tmpY = 0.0;        double drawX = 0.0, drawY = 0.0;        for(int i=0; i<1081; ++i)        {            theta = (i/4.0+130)*M_PI/180;            rho = RadarVector.at(index).data[i]*Zoom;            x =  rho*cos(theta);            y =  rho*sin(theta);            tmpX = x;            tmpY = y;            double rad_psi = DEG2RAD(EulerVector.at(index).psi);            // 旋转            drawX = tmpX*cos(rad_psi) - tmpY*sin(rad_psi);            drawY = tmpX*sin(rad_psi) + tmpY*cos(rad_psi);            // 位移            drawX += PosVector.at(index).p_x * Zoom;            drawY += PosVector.at(index).p_y * Zoom;            drawX =  imgWidth/5*4 + drawX;            drawY =  imgHeight/2 - drawY;            // 当前位置坐标            double draw_pose_x = imgWidth/5*4 + PosVector.at(index).p_x*Zoom;            double draw_pose_y = imgHeight/2 - PosVector.at(index).p_y*Zoom;            int r = drawX;            int c = drawY;            // 绘制地图            if(r>0 && r<WIDTH && c>0 && c<HEIGHT)            {                int val = mapData.mapsum[r][c];                mapData.mapsum[r][c]--;                if(val < 0)                    val = 0;                desImage.setPixel(r,c,qRgb(val,val,val));            }            // 绘制运动轨迹            int pose_r = draw_pose_x;            int pose_c = draw_pose_y;            if(pose_r>0 && pose_r<WIDTH && pose_c>0 && pose_c<HEIGHT)            {                desImage.setPixel(pose_r,pose_c,qRgb(0,0,255));            }        }    }    else{        if(timer->isActive()){            timer->stop();            qDebug()<<"stop...";            // 生成图片            desImage.save("map_b.png");        }    }    painter.drawImage(0,0,desImage);    ++index;    Sleep(10);}

效果如下：