SLAM - 利用Qt绘制灰度地图

最新推荐文章于 2022-08-17 21:52:31 发布

爱爬山的IT小小鸟

最新推荐文章于 2022-08-17 21:52:31 发布

5604

分类专栏： Qt 其他数据结构与算法文章标签： slam Qt 灰度图

本文链接：https://blog.csdn.net/AnonymousRookie/article/details/51807590

版权

Qt 同时被 3 个专栏收录

2 篇文章 1 订阅

订阅专栏

其他

5 篇文章 4 订阅

订阅专栏

数据结构与算法

2 篇文章 0 订阅

订阅专栏

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