SLAM学习笔记（二）数据采集

最新推荐文章于 2023-03-29 22:49:58 发布
最新推荐文章于 2023-03-29 22:49:58 发布
阅读量2.3k 收藏 10
点赞数
分类专栏： 机器学习 文章标签： velodyne PCAP报文解析
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/zx3517288/article/details/78431425
版权

转载：http://m.blog.csdn.net/sunboyiris/article/details/75172025
激光型号：LiDAR-Velodyne——VLP16
在使用velodyne的时候需要解析PCAP文件，将PCAP格式数据转为XYZ格式的点云数据,代码如下：

/ readpcap.cpp : 定义控制台应用程序的入口点。
//
#define _CRT_SECURE_NO_DEPRECATE
#define _CRT_SECURE_NO_WARNINGS
#include "pcap.h"
#include "stdio.h"
#include "math.h"
#include<stdlib.h>
#include<conio.h>
#include "velodyneptk.h"
#define LINE_LEN 16

int Azimuth_[12];               //原始值
float Azimuth_Value_[12];
int Distance_[12][32];          //原始值
float Distance_Value_[12][32];
int Atten_[12][32];     //原始值
Usefulmessage UsefulData;
int framecount;
int frameint;
//计算时间戳函数
float Timeoffsetvec[384];
float lasersinvec[384];
float lasercosvec[384];
void Timeoffsetfun()
{
    for (int i = 0; i < 24; i++)
    {
        for (int j = 0; j < 16; j++)
        {
            Timeoffsetvec[i * 16 + j] = i*55.296 + j*2.304;
            lasersinvec[i * 16 + j] = LASER_SIN[j];
            lasercosvec[i * 16 + j] = LASER_COS[j];
        }
    }
}

void dispatcher_handler(u_char *, const struct pcap_pkthdr *, const u_char *);


//byte转int  Azimuth
int bytes2ToInt(byte* bytes)
{
    int addr = bytes[0] & 0xFF;
    addr |= (bytes[1]<<8 & 0xFF00);
    return addr;
}

int bytes1ToInt(byte* bytes)
{
    int addr = bytes[0] & 0xFF;
    return addr;
}

//byte转int  Azimuth
long int bytes4ToInt(byte* bytes)
{
    long int addr = bytes[0] & 0xFF;
    addr |= (bytes[1] << 8 & 0xFF00);
    addr |= ((bytes[2] << 16) & 0xFF0000);
    addr |= ((bytes[3] << 24) & 0xFF000000);
    return addr;
}

float stamptimecount = 0;
void UDPtoXYZfun(Usefulmessage data);
void UDPtoXYZfunALL(Usefulmessage data);
errno_t err;

int _tmain(int argc, _TCHAR* argv[])
{
    pcap_t *fp;
    char errbuf[PCAP_ERRBUF_SIZE];
    Usefulmessage UsefulData;
    Timeoffsetfun();
    framecount = 0;

    fp = pcap_open_offline("1.pcap",errbuf);
    pcap_loop(fp, 0, dispatcher_handler, NULL);
    pcap_close(fp);
    return 0;
}

void dispatcher_handler(u_char *temp1,
    const struct pcap_pkthdr *header,
    const u_char *pkt_data)
{
    u_int it = 0;

    (VOID*)temp1;

    //保存数据
    char fn[20];
    PointXYZ point;
    FILE *fp;


    long int ustime = header->ts.tv_usec;
    printf("%ld:%ld (%ld)\n", header->ts.tv_sec, header->ts.tv_usec, header->len);
    if (header->len==1248)
    {
        byte timestampl[4];
        byte factoryl[2];

        byte lo[1248];
        memcpy(&lo, pkt_data, 1248);
        memcpy(×tampl, pkt_data + 42 + 12 * 100, 4);
        memcpy(&factoryl, pkt_data + 42 + 12 * 100 + 4, 2);
        //float fValuet = *((float*)×tampl); //系统时间
        int fValue1 = *((float*)&factoryl[0]);
        long int fValue = bytes4ToInt(timestampl);
        if (stamptimecount == 0)
        {
            stamptimecount = fValue;
        }
        if ((fValue - stamptimecount) >= 100000)
        {
            stamptimecount = fValue;
            frameint++;
        }
        /保存数据
        sprintf_s(fn, "%05d.txt", frameint);
        //err  = fopen_s( &stream, "crt_fopen_s.c", "r" )) !=0
        if ((fp = fopen(fn, "a")) == NULL)
        {
            printf("Create File failure 1");
            fclose(fp);
            //exit(1);
        }

        //read data
        byte datal[12][100];
        int packet_size = 100;
        int azimuth;
        float distance;
        int passway;
        for (int i = 0; i < 12; i++)
        {
            memcpy(&datal[i], pkt_data + 42 + i * 100, packet_size);
            BYTE b[2];
            BYTE b1[1];
            memcpy(&b, pkt_data + 42 + i * 100 + 2, 2);
            azimuth = bytes2ToInt(b);
            UsefulData.JIAODU_[i] = azimuth;
            UsefulData.JIAODU2_[i] = azimuth*0.01;
            UsefulData.Timesec = header->ts.tv_sec;
            //printf("%f\n", UsefulData.JIAODU2_[i]);
            UsefulData.TimeStamp = fValue;
            for (int j = 0; j < 32; j++)
            {
                memcpy(&b, pkt_data + 42 + i * 100 + 4 + j * 3, 2);
                memcpy(&b1, pkt_data + 42 + i * 100 + 4 + j * 3 + 2, 1);
                distance = float(bytes2ToInt(b))*0.002f;

                passway = bytes1ToInt(b1);
                if (distance<0.05)
                {
                    UsefulData.JULI_[i][j] = 0;
                    UsefulData.PointPos[i][j] = i * 32 + j;
                    //printf("%d  ", UsefulData.PointPos[i][j]);
                }
                else
                {
                    UsefulData.JULI_[i][j] = distance;
                    UsefulData.PointPos[i][j] = i * 32 + j;
                }

                UsefulData.PASSEGEWAY_[i][j] = passway;

                UsefulData.TimeOffset[i][j] = Timeoffsetvec[i * 32 + j] + header->ts.tv_usec;  //时间戳
                UsefulData.SIN_[i][j] = lasersinvec[i * 32 + j];
                UsefulData.COS_[i][j] = lasercosvec[i * 32 + j];
                //  printf("%f ", UsefulData.PASSEGEWAY_[i][j]);


            }

        }
        //经度赋值
        for (int i1 = 0; i1 < 12; i1++)
        {
            for (int k = 0; k < 32; k++)
            {
                if (k < 16)
                {
                    UsefulData.Azimuth[i1][k] = UsefulData.JIAODU2_[i1];// +LASER_vert_correction[k];
                }
                else if (k >= 16)
                {
                    if (i1 < 11)//前11帧
                    {
                        if (UsefulData.JIAODU2_[i1 + 1] < UsefulData.JIAODU2_[i1])
                        {
                            UsefulData.JIAODU2_[i1 + 1] += 360.0;
                            float azimuth2 = UsefulData.JIAODU2_[i1] + (UsefulData.JIAODU2_[i1 + 1] - UsefulData.JIAODU2_[i1]) / 2.0;
                            if (azimuth2 > 360)// 角度变化了
                            {
                                azimuth2 -= 360;
                                UsefulData.Azimuth[i1][k] = azimuth2;// +LASER_vert_correction[k - 16];
                            }
                            else
                            {
                                UsefulData.Azimuth[i1][k] = azimuth2;// +LASER_vert_correction[k - 16];
                            }
                        }
                        else
                        {
                            float azimuth4 = UsefulData.JIAODU2_[i1] + (UsefulData.JIAODU2_[i1 + 1] - UsefulData.JIAODU2_[i1]) / 2.0;
                            UsefulData.Azimuth[i1][k] = azimuth4;// +LASER_vert_correction[k - 16
                        }
                    }
                    else if (i1 == 11)//最后一帧
                    {
                        float azimuth3 = UsefulData.JIAODU2_[i1] + 0.2;
                        if (azimuth3 > 360)
                        {
                            azimuth3 -= 360;
                        }
                        else
                        {
                        }
                        UsefulData.Azimuth[i1][k] = azimuth3;//;+LASER_vert_correction[k - 16];
                    }
                }
                point.x = UsefulData.JULI_[i1][k] * UsefulData.COS_[i1][k] * sin(UsefulData.Azimuth[i1][k] / 180.0*3.1415926);
                point.y = UsefulData.JULI_[i1][k] * UsefulData.COS_[i1][k] * cos(UsefulData.Azimuth[i1][k] / 180.0*3.1415926);
                point.z = UsefulData.JULI_[i1][k] * UsefulData.SIN_[i1][k];
                point.r = UsefulData.PASSEGEWAY_[i1][k];
                point.tus = UsefulData.TimeOffset[i1][k];
                point.tsec = UsefulData.Timesec;
                if ((point.x == 0) && (point.y == 0) && (point.z == 0) || (UsefulData.JULI_[i1][k] <= 0.05))
                {
                }
                else
                {
                    //X  Y  Z  Azimuth Distance Laser_ID 
                    fprintf(fp, "%f %f %f %f %f %d ", point.x, point.y, point.z, UsefulData.Azimuth[i1][k], UsefulData.JULI_[i1][k], UsefulData.PointPos[i1][k] % 16);
                    fprintf(fp, "%f %f %ld %ld\n ", UsefulData.PASSEGEWAY_[i1][k], point.tus,  header->ts.tv_sec,ustime);
                }
            }

        }
        fclose(fp);

    }

    printf("\n\n");
}

void UDPtoXYZfunALL(Usefulmessage data)
{
    PointXYZ point[384];
    FILE *fp;
    if ((fp = fopen("all.txt", "a")) == NULL)
    {
        printf("Create File failure");
        //getch();
        exit(1);
    }
    for (int i = 0; i < 12; i++)
    {
        for (int j = 0; j < 32; j++)
        {
            point[i * 32 + j].x = data.JULI_[i][j] * data.COS_[i][j] * sin(data.Azimuth[i][j] / 180.0*3.1415926);
            point[i * 32 + j].y = data.JULI_[i][j] * data.COS_[i][j] * cos(data.Azimuth[i][j] / 180.0*3.1415926);
            point[i * 32 + j].z = data.JULI_[i][j] * data.SIN_[i][j];
            point[i * 32 + j].r = data.PASSEGEWAY_[i][j];
            point[i * 32 + j].tus = data.TimeOffset[i][j];
            point[i * 32 + j].tsec = data.Timesec;
            if ((point[i * 32 + j].x == 0) && (point[i * 32 + j].y == 0) && (point[i * 32 + j].z == 0) || (data.JULI_[i][j] <= 0.05))
            {
            }
            else
            {
                //X  Y  Z  Azimuth Distance Laser_ID 
                fprintf(fp, "%f %f %f %f %f %d ", point[i * 32 + j].x, point[i * 32 + j].y, point[i * 32 + j].z, data.Azimuth[i][j], data.JULI_[i][j], data.PointPos[i][j] % 16);
                fprintf(fp, "%f %f %f %f \n ", data.PASSEGEWAY_[i][j], point[i * 32 + j].tus, data.TimeStamp, data.Timesec);

                //fprintf(fp, "%f %f %f \n", point[i * 32 + j].x, point[i * 32 + j].y, point[i * 32 + j].z);

            }
        }
    }
    fclose(fp);
}


void UDPtoXYZfun(Usefulmessage data)
{
    PointXYZ point[384];
    FILE *fp;
    if ((fp = fopen("my.txt", "a")) == NULL)
    {
        printf("Create File failure");
        //getch();
        exit(1);
    }   
    for (int i = 0; i < 12; i++)
    {
        for (int j = 0; j < 32; j++)
        {
            point[i * 32 + j].x = data.JULI_[i][j]*data.COS_[i][j]*sin(data.Azimuth[i][j]/180.0*3.1415926);
            point[i * 32 + j].y = data.JULI_[i][j]*data.COS_[i][j]*cos(data.Azimuth[i][j]/180.0*3.1415926);
            point[i * 32 + j].z = data.JULI_[i][j] * data.SIN_[i][j];
            point[i * 32 + j].r = data.PASSEGEWAY_[i][j];
            point[i * 32 + j].tus = data.TimeOffset[i][j];
            point[i * 32 + j].tsec = data.Timesec;
            if ((point[i * 32 + j].x == 0) && (point[i * 32 + j].y == 0) && (point[i * 32 + j].z == 0)||(data.JULI_[i][j]<=0.05))
            {
            }
            else
            {

                //fprintf(fp, "%f %f %f %f %f %d\n", point[i * 32 + j].x, point[i * 32 + j].y, point[i * 32 + j].z,data.Azimuth[i][j],data.JULI_[i][j],data.PointPos[i][j]%16);
                fprintf(fp, "%f %f %f \n", point[i * 32 + j].x, point[i * 32 + j].y, point[i * 32 + j].z);

            }
        }
    }
    fclose(fp);
}
走过_冬天
关注
  • 0
    点赞
  • 踩
  • 10
    收藏
    觉得还不错? 一键收藏
  • 6
    评论
专栏目录
slamdata Unix/Linux安装程序
01-06
slamdata是一个针对包括mongodb在内的NoSQL的数据可视化工具
slam学习笔记
02-05
slam学习知识，深度视觉的问答笔记，资源均来源于网络
SLAM学习之路-使用RS雷达获取点云数据(记录详细步骤)
weixin_43990795的博客
11-24 1857
1.ros安装 由于在ubantu写的这篇文章，所以不好截图，但是会比较详细。 本人是18.04的版本，这里给出师兄的16.04的安装，https://blog.csdn.net/ustcyr/article/details/105172760只要修改knetic成melodic就行，和自己的ubantu版本对应就好了。 这里估计会有rosdep update 报错，要么你挂梯子，要不就参考我的链接修改几个文件，我都标注了修改哪里，添加哪里，很简单。https://blog.csdn.net/weixin_
SLAM资源收集
damifeng2018的博客
08-04 1607
http://www.voidcn.com/blog/scliu12345/article/p-5694464.html SLAM课程： 1、http://ais.informatik.uni-freiburg.de/teaching/ws15/mapping/  （https://www.youtube.com/watch?v=U6vr3iNrwRA&index=1&
slam数据集收集
qq_38497150的博客
12-26 348
http://www.cnblogs.com/yhlx125/p/5609998.html
SLAM 数据集
05-19 9220
SLAM benchmarking.  http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets.phpKITTI SLAM dataset.  http://www.cvlibs.net/datasets/kitti/eval_odometry.php. 包括 单目视觉 ,双目视觉, velodyne, POS 轨迹 O
Jetson NX采集原始SLAM数据的一种思路
weixin_41648131的博客
02-14 430
Jetson NX采集原始SLAM数据的一种思路，省去了用Putty之类的远程登陆工具，进入Jetson NX终端输入命令
SLAM学习笔记-超有用
05-02
关于SLAM的学习笔记，对刚入门的童鞋可能帮助比较大，分享一下！
二维激光雷达SLAM数据集
04-22
二维激光雷达SLAM数据集
ROS与SLAM从零开始学习笔记（一）——ROS概述与环境搭建
01-20
写在前面：笔者最近在做一个基于ROS平台的小项目，也算是从零开始学ROS，在这里写下一些学习时的笔记。 笔者学习参考：古月居 一、ROS环境安装 ROS历经多个版本的升级，目前正不断适用于机器人领域的研究与控制，ROS...
python版pcap转txt工具
11-01
分析网络协议时会用到pcap文件,但是使用其他的工具无法做到信息汇总,或者只看到其中关心的消息 这个工具可以将pcap文件的中的关键信息解析出来 格式如下 源IP 目标IP 源端口 目标端口 协议类型 时间 包大小格式化以后输出出来 一共输出两个文件 一个txt文件 一个sql文件,方便存入到数据库中 源IP: 111.10.121.157 目标IP:123.123.123.123 协议类型:ICMP 时间:2018-10-21 13:56:45 包大小:74 源IP: 111.10.121.157 目标IP:123.123.123.123 协议类型:ICMP 时间:2018-10-21 13:56:47 包大小:74 源IP: 10.19.255.7 目标IP:123.123.123.123 协议类型:TCP 源端口:80 目标端口 :54954 时间:2018-10-21 13:56:47 包大小:54 源IP: 10.19.255.7 目标IP:123.123.123.123 协议类型:TCP 源端口:80 目标端口 :54956 时间:2018-10-21 13:56:47 包大小:74 源IP: 10.19.255.7 目标IP:123.123.123.123 协议类型:TCP 源端口:80 目标端口 :54956 时间:2018-10-21 13:56:47 包大小:66 源IP: 10.19.255.7 目标IP:123.123.123.123 协议类型:TCP 源端口:80 目标端口 :54956 时间:2018-10-21 13:56:47 包大小:312
slam笔记.zip
07-29
原创视频以及一些开源的资料，视频包括代码讲解，论文分析，原理分析，侧重原理
【工程实践】飞马SLAM100三维数据处理
ZhiyangStudy的博客
03-29 2039
SLAM三维数据处理流程记录。
Linux内核获取系统时间
u013053075的博客
04-24 3712
在Linux内核中，常常使用do_gettimeofday()函数来得到精确的系统运行时间，尤其在嵌入式中非常常见。 很多程序运行，不需要获取到年月日等信息，但是需要获取高精度的系统时间，可以使用这个函数。 函数功能和C标准库中gettimeofday()用法相同。 下面代码拿去使用吧。 #include <linux/time.h> unsigned int system...
velodyne：激光雷达pcap文件格式及写入、数据解析
热门推荐
通哈膨胀哈哈哈 的博客
06-21 1万+
一 基本格式：   文件头 数据包头 数据报数据包头数据报......二、文件头：      文件头结构体 sturct pcap_file_header {      DWORD           magic;      DWORD           version_major;      DWORD           version_minor;      DWORD          ...
Velodyne 32E pcap包GPS时间戳解析
08-23 1145
1. pcap header中的时间戳解析 2. pcap data中gps时间解析 3. gps时间戳合成
C++时间工具类——纳秒，微秒，毫秒，秒，日期
老人头的博客
09-04 6025
1、找一个比较全的时间工具类太难了，没人总结啊（适用于linux）。 #include <ctime> #include <stdint.h> #include <iostream> #include <string> #include <sys/time.h> using std::string; /** * Linux高精度...
pcap数据包转换成netflow
weizheng1000的专栏
11-22 3999
使用命令： nprobe -i 文件路径 -P ./ -D t 命令执行后nprobe会解析pcap包以netflow的格式存储在本地 netflow样本：
Linux文件夹/文件基本操作
08-07 599
删除目录、文件 rm(remove)功能说明：删除文件或目录。 语　　法：rm [-dfirv][–help][–version][文件或目录…] 补充说明：执行rm指令可删除文件或目录，如欲删除目录必须加上参数”-r”，否则预设仅会删除文件。 参　　数： 　-d或–directory 　直接把欲删除的目录的硬连接数据删成0，删除该目录。 　-f或–force 　强制删除文件或目录。
无人驾驶怎么应用SLAM算法实时采集数据VO进行特征匹配
最新发布
06-10
无人驾驶应用SLAM算法实时采集数据VO进行特征匹配的基本步骤如下： 1. 采集数据：使用激光雷达、摄像头等传感器实时采集车辆周围的环境数据。 2. 运行VO算法：使用VO算法（Visual Odometry）对采集到的图像进行处理，提取出关键帧、特征点等信息。 3. 运行SLAM算法：使用SLAM算法（Simultaneous Localization and Mapping）对车辆所在的位置和周围环境进行建模。 4. 特征匹配：将VO算法提取出的特征点与SLAM算法建模得到的地图进行匹配，计算出车辆所在的位置。 5. 实时更新地图：根据车辆实时的位置和周围环境变化，实时更新SLAM算法建立的地图。 通过以上步骤，可以实现无人驾驶车辆的自主定位和导航，从而实现自动驾驶功能。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 6
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值