1、下载CC源码,下载CCoreLib源码,一起编译,生成以下4个库,拷贝对应的lib、dll、h文件备用
QCC_GL_LIB、QCC_DB_LIB、CCCoreLib、CC_FBO_LIB
2、工程定义宏
CC_CORE_LIB_USES_FLOAT
CC_CORE_LIB_USES_QT_CONCURRENT
NOMINMAX
WIN32_LEAN_AND_MEAN
_CRT_SECURE_NO_WARNINGS
__STDC_LIMIT_MACROS
3、工程链接以上4个lib
4、源代码
#ifndef SIMPLEGLWIDGET_H
#define SIMPLEGLWIDGET_H
#include <QWidget>
#include <ccHObject.h>
#include <QTimer>
#include <sptreader.hpp>
#include <gradcolor.hpp>
#include "datareader.h"
#include "VelodyneHdl.hpp"
class ccGLWindow;
namespace Ui {
class SimpleGlWidget;
}
class SimpleGlWidget : public QWidget
{
Q_OBJECT
public:
SimpleGlWidget(QWidget *parent = nullptr);
~SimpleGlWidget();
public:
void addToDB(ccHObject* entity);
void updateTrans(ccHObject* obj, CCVector3 povit, CCVector3 rot, CCVector3 pos);
void OnTimerTick();
void Demo(std::vector<ldrcali>& v);
void onCaliChange();
signals :
void ManualUpdate();
public:
//! Associated GL context
ccGLWindow* m_glWindow;
ccHObject* pRoot;
ccHObject* pCloud;
ccHObject* pCone;
ccHObject* pTrack;
QTimer updateTimer;
CCVector3d global_offset;
std::map<char, ccColor::Rgb> _color;
velodyne::HDL::HDLDecoder _dec;
private:
Ui::SimpleGlWidget *ui;
};
#endif // SIMPLEGLWIDGET_H
#include "simpleglwidget.h"
#include "ui_simpleglwidget.h"
#include <ccHObjectCaster.h>
#include <ccPointCloud.h>
#include <ccPlane.h>
#include <ccCone.h>
#include <ccGLWidget.h>
#include <QVBoxLayout>
#include <QDebug>
SimpleGlWidget::SimpleGlWidget(QWidget *parent) :
QWidget(parent),
ui(new Ui::SimpleGlWidget)
{
ui->setupUi(this);
//insert GL window in a vertical layout
{
QVBoxLayout* verticalLayout = new QVBoxLayout(ui->GLframe);
verticalLayout->setSpacing(0);
const int margin = 10;
verticalLayout->setContentsMargins(margin, margin, margin, margin);
bool stereoMode = QSurfaceFormat::defaultFormat().stereo();
QWidget* glWidget = nullptr;
CreateGLWindow(m_glWindow, glWidget, stereoMode);
//assert(m_glWindow && glWidget);
verticalLayout->addWidget(glWidget);
pRoot = new ccHObject("root");
pCloud = ccHObject::New(CC_TYPES::POINT_CLOUD, "PointCloud");
pCone = ccHObject::New(CC_TYPES::CONE, "Camera1");
pTrack = ccHObject::New(CC_TYPES::POINT_CLOUD, "Track");
pRoot->addChild(pCloud);
pRoot->addChild(pCone);
pRoot->addChild(pTrack);
}
gradcolor gradd;
gradd.calc();
for (int var = 0; var < 255; ++var) {
_color.insert(std::make_pair(var, ccColor::Rgb(
gradd.grad[var].r,
gradd.grad[var].g,
gradd.grad[var].b)));
}
connect(&updateTimer, &QTimer::timeout, this, &SimpleGlWidget::OnTimerTick);
connect(ui->pushButton, &QPushButton::clicked, this, [&](){
_dec.nextframe();
});
/*
connect(ui->doubleSpinBox, &QDoubleSpinBox::valueChanged, this, [&](double dv){
_dec.nextframe();
});
*/
connect(ui->doubleSpinBox, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged),
this, [&](double v){ onCaliChange(); });
connect(ui->doubleSpinBox_2, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged),
this, [&](double v){ onCaliChange(); });
connect(ui->doubleSpinBox_3, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged),
this, [&](double v){ onCaliChange(); });
connect(ui->doubleSpinBox_4, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged),
this, [&](double v){ onCaliChange(); });
connect(ui->doubleSpinBox_5, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged),
this, [&](double v){ onCaliChange(); });
connect(ui->doubleSpinBox_6, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged),
this, [&](double v){ onCaliChange(); });
connect(ui->pushButton_2, &QPushButton::clicked, this, [&](){ m_glWindow->setView(CC_VIEW_ORIENTATION::CC_TOP_VIEW); });
connect(ui->pushButton_3, &QPushButton::clicked, this, [&](){ m_glWindow->setView(CC_VIEW_ORIENTATION::CC_FRONT_VIEW); });
connect(ui->pushButton_4, &QPushButton::clicked, this, [&](){ m_glWindow->setView(CC_VIEW_ORIENTATION::CC_RIGHT_VIEW); });
connect(this, &SimpleGlWidget::ManualUpdate, this, [&]{
m_glWindow->redraw(true, false);
});
updateTimer.setInterval(100);
updateTimer.start();
std::thread* pThread = new std::thread([&](){
_dec.init();
CCVector3 xyz;
ccPointCloud* cloud = ccHObjectCaster::ToPointCloud(pTrack);
cloud->reset();
global_offset.x = _dec.posVec[0].e;
global_offset.y = _dec.posVec[0].n;
global_offset.z = _dec.posVec[0].u;
for (unsigned int i = 0; i < _dec.posVec.size(); ++ i){
xyz.x = _dec.posVec[i].e - global_offset.x;
xyz.y = _dec.posVec[i].n - global_offset.y;
xyz.z = _dec.posVec[i].u - global_offset.z;
cloud->addPoint(xyz);
}
/*
if (cloud->resizeTheRGBTable(true)){
for (unsigned int i = 0; i < cloud->size(); ++ i){
ccColor::Rgb rgb = ccColor::Rgb(0,255,0);
cloud->setPointColor(i, rgb);
}
}
*/
cloud->setColor(ccColor::red);
cloud->showColors(true);
_dec.callback = [&](std::vector<ldrcali>& v1){
Demo(v1);
emit ManualUpdate();
};
std::vector<std::string> pcaps;
pcaps.push_back("D:/testdir/pcap/K01/10011105180318133056.pcap");
//pcaps.push_back("D:/testdir/pcap/K01/10011105180318133756.pcap");
_dec.singleresolve(pcaps);
});
pThread->detach();
}
SimpleGlWidget::~SimpleGlWidget()
{
delete ui;
}
void SimpleGlWidget::onCaliChange(){
static std::chrono::steady_clock::time_point tmstart = std::chrono::steady_clock::now();
static std::chrono::duration<double, std::milli> elapsed = std::chrono::steady_clock::now() - tmstart;
elapsed = std::chrono::steady_clock::now() - tmstart;
if (elapsed.count() < 50)
return;
tmstart = std::chrono::steady_clock::now();
double pr[] = { ui->doubleSpinBox->value(),ui->doubleSpinBox_2->value(),ui->doubleSpinBox_3->value()};
double pt[] = { ui->doubleSpinBox_4->value(),ui->doubleSpinBox_5->value(),ui->doubleSpinBox_6->value()};
_dec.update_cali(pr, pt);
}
void SimpleGlWidget::OnTimerTick(){
//m_glWindow->redraw(true, false);
}
void SimpleGlWidget::Demo(std::vector<ldrcali>& v){
ccCone* cone = ccHObjectCaster::ToCone(pCone);
cone->setDrawingPrecision(4);
cone->setHeight(10);
cone->setBottomRadius(3);
cone->setTopRadius(0);
cone->setTempColor(ccColor::red);
ccPointCloud* cloud = ccHObjectCaster::ToPointCloud(pCloud);
if (true){
cloud->reset();
CCVector3 xyz, org;
for (unsigned int i = 0; i < v.size(); ++ i){
xyz.x = v[i].enu[0] - global_offset.x;
xyz.y = v[i].enu[1] - global_offset.y;
xyz.z = v[i].enu[2] - global_offset.z;
cloud->addPoint(xyz);
}
}
if (cloud->resizeTheRGBTable()){
#pragma omp parallel for
for (int i = 0; i < cloud->size(); ++ i){
cloud->setPointColor(i, _color[v[i].intensity]);
}
}
//cloud->setTempColor(ccColor::green);
cloud->showColors(true);
static int nxx = 0;
if (nxx == 0){
nxx = 1;
addToDB(pRoot);
}
//CCVector3 Cd = {0, 0, 0};
//updateTrans(pCone, cone->getTopCenter(), CCVector3(-90,45,0), Cd);
}
void SimpleGlWidget::addToDB(ccHObject* entity)
{
assert(entity && m_glWindow);
entity->setDisplay_recursive(m_glWindow);
ccHObject* currentRoot = m_glWindow->getSceneDB();
if (currentRoot)
{
//already a pure 'root'
if (currentRoot->isA(CC_TYPES::HIERARCHY_OBJECT))
{
currentRoot->addChild(entity);
}
else
{
ccHObject* root = new ccHObject("root");
root->addChild(currentRoot);
root->addChild(entity);
m_glWindow->setSceneDB(root);
}
}
else
{
m_glWindow->setSceneDB(entity);
}
}
void SimpleGlWidget::updateTrans(ccHObject* obj, CCVector3 povit, CCVector3 r, CCVector3 t)
{
if (!obj)
{
assert(false);
return;
}
CCVector3 C = povit;
//shall we transform (translate and / or rotate) the plane?
ccGLMatrix trans;
bool needToApplyRot = true;
bool needToApplyTrans = true;
if (needToApplyTrans)
{
trans.setTranslation(-C);
}
if (needToApplyRot)
{
static ccGLMatrix lastRot, rotation;
static ccGLMatrix rotX, rotY, rotZ;
static CCVector3 axis[3] = {CCVector3(1, 0, 0),CCVector3(0, 1, 0),CCVector3(0, 0, 1)};
static CCVector3 t3D = CCVector3(0, 0, 0);
rotX.initFromParameters( CCCoreLib::DegreesToRadians( r.x ), axis[0], t3D ); //plunge
rotY.initFromParameters( CCCoreLib::DegreesToRadians( r.y ), axis[1], t3D ); //plunge
rotZ.initFromParameters( CCCoreLib::DegreesToRadians( r.z ), axis[2], t3D ); //plunge
rotation = rotZ * rotY * rotX;
trans = lastRot.inverse() * rotation * trans;
lastRot = rotation;
}
if (needToApplyTrans)
{
trans.setTranslation(trans.getTranslationAsVec3D() + t);
}
if (needToApplyRot || needToApplyTrans)
{
obj->applyGLTransformation_recursive(&trans);
}
}
VelodyneHDL.hpp
#pragma once
#include "PacketDecoder.h"
#include "pcaploader.hpp"
#include "datatypes.hpp"
#include "datareader.h"
#include <iostream>
#include <QDir>
#include <QFileInfoList>
#include <QDebug>
#include <QMutex>
#include <Eigen/Dense>
#include <cmath>
#include <algorithm>
#include <omp.h>
#include <time.h>
#include <chrono>
namespace velodyne {
namespace HDL {
class HDLDecoder
{
public:
HDLDecoder() {
vlines = 32;
memset(caliRot, 0, sizeof(double) * 3);
memset(caliOffT, 0, sizeof(double) * 3);
memset(veloxml, 0, sizeof(veloxml));
caliRot[0] = -43.1128;
caliRot[1] = 0.788177;
caliRot[2] = 91.5099;
caliOffT[0] = 0;
caliOffT[1] = 0;
caliOffT[2] = 0;
/*
qDebug() << omp_get_num_procs();
qDebug() << "before get openMP max threads: " << omp_get_max_threads() << endl;
qDebug() << "before set openMP threads: " << omp_get_num_threads() << endl;
int set_thread = 10;
omp_set_num_threads(8);
qDebug() << "after set openMP threads: " << omp_get_num_threads() << endl;
#pragma omp parallel
{
qDebug() << "parallel:" << omp_get_thread_num() << endl;
#pragma omp for
for (int i = 0; i < 4 ; ++ i){
qDebug() << "Hello" << ", I am Thread " << omp_get_thread_num() << endl;
}
}
*/
}
~HDLDecoder(){
}
public:
bool init(){
datareader dr;
dr.loadpos_v1("D:\\testdir\\pcap\\K01\\1001-1-105-180318.txt", posVec);
if (posVec.size() < 10)
return false;
return true;
}
void update_cali(double* r, double* t){
std::chrono::steady_clock::time_point tmstart = std::chrono::steady_clock::now();
for (int i = 0; i < 3; ++i) {
caliRot[i] = r[i];
caliOffT[i] = t[i];
}
Eigen::AngleAxisd rx_ldr,ry_ldr,rz_ldr;
rx_ldr = Eigen::AngleAxisd(caliRot[0] / 180.0 * M_PI, Eigen::Vector3d::UnitX());
ry_ldr = Eigen::AngleAxisd(caliRot[1] / 180.0 * M_PI, Eigen::Vector3d::UnitY());
rz_ldr = Eigen::AngleAxisd(caliRot[2] / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
cali_R = rz_ldr * ry_ldr * rx_ldr;
cali_T << caliOffT[0], caliOffT[1], caliOffT[2];
#pragma omp parallel
{
Eigen::AngleAxisd imuR;
Eigen::Vector3d imuT, ldrT, lasT, Axis;
#pragma omp for
for (int i = 0; i < ldrcalivec.size(); i++) {
Axis <<ldrcalivec[i].angleaxis[0],ldrcalivec[i].angleaxis[1],ldrcalivec[i].angleaxis[2];
imuR = Eigen::AngleAxisd(ldrcalivec[i].angleaxis[3], Axis);
imuT << ldrcalivec[i].imupos[0], ldrcalivec[i].imupos[1], ldrcalivec[i].imupos[2];
ldrT << ldrcalivec[i].xyz[0], ldrcalivec[i].xyz[1], ldrcalivec[i].xyz[2];
lasT = imuR * (cali_R.inverse() * ldrT + cali_T) + imuT;
ldrcalivec[i].enu[0] = lasT[0];
ldrcalivec[i].enu[1] = lasT[1];
ldrcalivec[i].enu[2] = lasT[2];
//std::cout << "Hello" << ", I am Thread " << omp_get_thread_num() << std::endl;
}
}
std::chrono::duration<double, std::milli> elapsed = std::chrono::steady_clock::now() - tmstart;
char psz[521] = "";
sprintf(psz, "(%s) time span %.3f/s", "calc cost", elapsed.count() * 0.001);
qDebug() << QString::fromLocal8Bit(psz);
tmstart = std::chrono::steady_clock::now();
qDebug() << ldrcalivec.size();
if (callback != nullptr)
callback(ldrcalivec);
elapsed = std::chrono::steady_clock::now() - tmstart;
sprintf(psz, "(%s) time span %.3f/s", "show cost", elapsed.count() * 0.001);
qDebug() << QString::fromLocal8Bit(psz);
}
void nextframe(){
waitlock = false;
}
bool singleresolve(std::vector<std::string> pszpcap) {
int packets = 0, points = 0;
double lasttime = 0, currenttime = 0;
try
{
//包数据长度
int dataLength = 0;
//用于获取Velodyne 554包内激光时间
char pszgprmc[256] = "";
char pszhour[4] = "";
int ngprmchour = -1;
//PacketDecoder 参数
std::deque<PacketDecoder::HDLFrame> frames;
PacketDecoder::HDLFrame latest_frame;
//临时点
ldrcali _xyzcali;
PosTrack ppt;
Eigen::AngleAxisd rx,ry,rz,R;
Eigen::Vector3d ldrT, imuT, lasT;
//解析包
PacketDecoder pd;
pd.velolines = vlines;
pd.SetCorrectionsFile(veloxml);
pd.SetMaxNumberOfFrames(100);
for (int pcapid = 0; pcapid < pszpcap.size(); ++ pcapid){
pcaploader::pcappackets(pszpcap[pcapid].c_str(), [&](const unsigned char* p, unsigned int* n) {
if (*n == 1248 && ngprmchour >= 0) {
packets ++ ;
dataLength = 1206;
pd.DecodePacket((const char*)p + 42, &dataLength);
frames = pd.GetFrames();
if (/*pd.GetLatestFrame(&latest_frame)*/frames.size() > 0) {
while (!frames.empty()) {
latest_frame = frames.front();
frames.pop_front();
for (size_t i = 0; i < latest_frame.x.size(); i+=1)
{
points++;
if (latest_frame.laser_id[i] == 15 && latest_frame.distance[i] > 0 && latest_frame.distance[i] < 110) {
if (latest_frame.intensity[i] < 4)
continue;
currenttime = ngprmchour * 3600 + 18 + latest_frame.ms_from_top_of_hour[i] * 1e-6;
ppt.wsec = currenttime;
long long idx = std::lower_bound(posVec.begin(), posVec.end(), ppt, [](const PosTrack& v1, const PosTrack& v2){
return v1.wsec <= v2.wsec;
}) - posVec.begin();
idx = (idx - 1) >= 0 ? (idx - 1) : 0;
rx = Eigen::AngleAxisd(posVec[idx].p / 180.0 * M_PI, Eigen::Vector3d::UnitX());
ry = Eigen::AngleAxisd(posVec[idx].r / 180.0 * M_PI, Eigen::Vector3d::UnitY());
rz = Eigen::AngleAxisd(-posVec[idx].y / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
R = rz * rx * ry;
_xyzcali.xyz[0] = latest_frame.x[i];
_xyzcali.xyz[1] = latest_frame.y[i];
_xyzcali.xyz[2] = latest_frame.z[i];
_xyzcali.angleaxis[0] = R.axis()[0];
_xyzcali.angleaxis[1] = R.axis()[1];
_xyzcali.angleaxis[2] = R.axis()[2];
_xyzcali.angleaxis[3] = R.angle();
_xyzcali.imupos[0] = posVec[idx].e;
_xyzcali.imupos[1] = posVec[idx].n;
_xyzcali.imupos[2] = posVec[idx].u;
_xyzcali.intensity = (latest_frame.intensity[i] * 2) % 255;
_xyzcali.srcid = latest_frame.laser_id[i];
ldrcalivec.emplace_back(std::move(_xyzcali));
}
}
/*
if ((currenttime - lasttime) > 18){
if (lasttime > 1){
update_cali(caliRot, caliOffT);
waitlock = true;
while (waitlock) {
Sleep(300);
}
ldrcalivec.clear();
qDebug() << "ldrcalivec.clear()";
}
lasttime = currenttime;
}
*/
}
pd.ClearFrames();
}
//pd.ClearFrames();
}
else if (*n == 554) {
memcpy(pszhour, p + 248 + 7, 2);
ngprmchour = atoi(pszhour);
}
});
}
update_cali(caliRot, caliOffT);
return true;
}
catch (const std::exception&)
{
}
return false;
}
public:
//点序列
std::vector<ldrcali> ldrcalivec;
std::function<void(std::vector<ldrcali>&)> callback;
Eigen::AngleAxisd cali_R;
Eigen::Vector3d cali_T;
double caliRot[3];
double caliOffT[3];
public:
std::vector<PosTrack> posVec;
private:
int vlines;
char veloxml[MAX_PATH];
bool waitlock;
};
}
}