c++(4) pangolin可视化多线程

 

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(3DTo3D)

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_BUILD_TYPE Release)


find_package(Pangolin REQUIRED)# 可视化工具库
include_directories(${Pangolin_INCLUDE_DIRS})



# OpenCV
# find_package(OpenCV REQUIRED)
# include_directories(${OpenCV_INCLUDE_DIRS})
 

# Sophus
# find_package(Sophus REQUIRED)
# include_directories(${Sophus_INCLUDE_DIRS})
     
# Eigen
# 不需要寻找 手动指定默认安装位置 只是一堆头文件 无需target_link_libraries
find_package(Eigen3 REQUIRED)
include_directories("/usr/local/include/eigen3")


# 编译节点
add_executable(v1_node main.cpp)

# 链接目标
# target_link_libraries(v1_node ${Pangolin_LIBRARIES} ${OpenCV_LIBS}   -lgomp)

target_link_libraries(v1_node ${Pangolin_LIBRARIES} )

main.cpp

#include <iostream>
#include <thread>
#include <queue>
#include <mutex>
#include <unistd.h> // For sleep function
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <vector>
using namespace std;

// 用于存储3D点的队列和互斥锁
vector<Eigen::Vector3d> pointQueue;
mutex queueMutex;

using namespace std;
using namespace Eigen;

 std::vector<Eigen::Vector3d> generateCircle3D(double radius, int numPoints) {
    std::vector<Eigen::Vector3d> circlePoints;

    for (int i = 0; i < numPoints; ++i) {
        double theta = 2.0 * M_PI * i / numPoints;
        double x = radius * std::cos(theta);
        double y = radius * std::sin(theta);
        double z = 0.0;  // 在平面内，z 始终为 0
        circlePoints.emplace_back(x, y, z);
    }

    return circlePoints;
}


// 主线程生成3D点的函数
void generatePoints() {
    int pointId = 0;


  

    double radius = 10;  // 圆的半径
    int numPoints = 10; // 生成的点的数量
    vector<Vector3d> source_points = generateCircle3D(radius, numPoints);



    while (true) {

        if (pointId>=source_points.size()){break;}
        // 模拟生成一个3D点
        Eigen::Vector3d point=source_points[pointId];
        // 加入队列
        queueMutex.lock();
        pointQueue.push_back(point);
        queueMutex.unlock();

        ++pointId;
        sleep(1); // 每秒生成一个点
    }
}

// 显示线程使用Pangolin显示和处理3D点
void displayPoints() {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
      pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
      pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
      .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
      .SetHandler(new pangolin::Handler3D(s_cam));

   
  while (!pangolin::ShouldQuit()) {
        // 清空缓冲区
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
        glLineWidth(2);

        // 处理队列中的所有点并绘制
        queueMutex.lock();


        
        if(pointQueue.size()>=2){  
            
            for (size_t i = 0; i < pointQueue.size() - 1; i++) {
                    Eigen::Vector3d point1 = pointQueue[i];
                   
                    Eigen::Vector3d point2 =  pointQueue[i+1];

                    cout<<point1<<"    "<< point2 <<endl;

                    glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truth
                    glBegin(GL_LINES);
            
                    glVertex3d(point1[0], point1[1], point1[2]);
                    glVertex3d(point2[0], point2[1], point2[2]);
                    glEnd();

                
           }
        }
        
        queueMutex.unlock();
        
    
        // 完成帧并交换缓冲区
        pangolin::FinishFrame();
        usleep(1000);   // sleep 5 ms
    }

    // 关闭Pangolin窗口
    pangolin::DestroyWindow("3D Points Display");
}

int main() {
    // 启动生成和显示线程
    thread generateThread(generatePoints);
    thread displayThread(displayPoints);

    // 等待线程结束
    generateThread.join();
    displayThread.join();

    return 0;
}

　　

　　

 

#include<iostream>

#include<thread>

#include<queue>

#include<mutex>

#include<unistd.h>// For sleep function

#include<Eigen/Core>

#include<pangolin/pangolin.h>

#include<vector>

using namespace std;

// 用于存储3D点的队列和互斥锁

vector<Eigen::Vector3d> pointQueue;

mutex queueMutex;

using namespace std;

using namespace Eigen;

std::vector<Eigen::Vector3d> generateCircle3D(double radius, int numPoints) {

std::vector<Eigen::Vector3d> circlePoints;

for (int i = 0; i < numPoints; ++i) {

double theta = 2.0 * M_PI * i / numPoints;

double x = radius * std::cos(theta);

double y = radius * std::sin(theta);

double z = 0.0; // 在平面内，z 始终为 0

circlePoints.emplace_back(x, y, z);

}

return circlePoints;

}

// 主线程生成3D点的函数

void generatePoints() {

int pointId = 0;

 

double radius = 10; // 圆的半径

int numPoints = 10; // 生成的点的数量

vector<Vector3d> source_points = generateCircle3D(radius, numPoints);

while (true) {

if (pointId>=source_points.size()){break;}

// 模拟生成一个3D点

Eigen::Vector3d point=source_points[pointId];

// 加入队列

queueMutex.lock();

pointQueue.push_back(point);

queueMutex.unlock();

++pointId;

sleep(1); // 每秒生成一个点

}

}

// 显示线程使用Pangolin显示和处理3D点

void displayPoints() {

// create pangolin window and plot the trajectory

pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);

glEnable(GL_DEPTH_TEST);

glEnable(GL_BLEND);

glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

pangolin::OpenGlRenderState s_cam(

pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),

pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)

);

pangolin::View &d_cam = pangolin::CreateDisplay()

.SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)

.SetHandler(new pangolin::Handler3D(s_cam));

 

while (!pangolin::ShouldQuit()) {

// 清空缓冲区

glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

d_cam.Activate(s_cam);

glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

glLineWidth(2);

// 处理队列中的所有点并绘制

queueMutex.lock();

 

if(pointQueue.size()>=2){

 

for (size_t i = 0; i < pointQueue.size() - 1; i++) {

Eigen::Vector3d point1 = pointQueue[i];

 

Eigen::Vector3d point2 = pointQueue[i+1];

cout<<point1<<" "<< point2 <<endl;

glColor3f(0.0f, 0.0f, 1.0f); // blue for ground truth

glBegin(GL_LINES);

 

glVertex3d(point1[0], point1[1], point1[2]);

glVertex3d(point2[0], point2[1], point2[2]);

glEnd();

 

}

}

 

queueMutex.unlock();

 

 

// 完成帧并交换缓冲区

pangolin::FinishFrame();

usleep(1000); // sleep 5 ms

}

// 关闭Pangolin窗口

pangolin::DestroyWindow("3D Points Display");

}

int main() {

// 启动生成和显示线程

thread generateThread(generatePoints);

thread displayThread(displayPoints);

// 等待线程结束

generateThread.join();

displayThread.join();

return 0;

}