CloudCompare3D显示库集成

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

  • 0
    点赞
  • 7
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

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

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

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值