lio-cpp/pangolinViewer.cpp

//
// Created by zx on 22-12-2.
//

#include "pangolinViewer.h"
PangolinViewer* PangolinViewer::viewer_= nullptr;

PangolinViewer* PangolinViewer::CreateViewer()
{
  if (viewer_== nullptr)
  {
    viewer_=new PangolinViewer();
  }
  return viewer_;
}
PangolinViewer::~PangolinViewer()
{
  if(thread_!= nullptr)
  {
    if (thread_->joinable())
      thread_->join();
    delete thread_;
    thread_= nullptr;
  }
}


PangolinViewer::PangolinViewer()
{
    thread_=new std::thread(&PangolinViewer::loop,this);
}

void PangolinViewer::Join()
{
  if(thread_!= nullptr)
  {
    if (thread_->joinable())
      thread_->join();
  }
}


void PangolinViewer::SetCallbacks(ViewerSpinOnceCallback spinOnce,StartLocateBtnCallback startBtn,
                  StopBtnCallback stopBtn,FreqChangedCallback freqChanged)
{
  spinOnce_callback_=spinOnce;
  startBtn_callback_=startBtn;
  stopBtn_callback_=stopBtn;
  freqChanged_callback_=freqChanged;
}

void PangolinViewer::Init()
{
  pangolin::CreateWindowAndBind("Main", 1280, 760);
  // 3D Mouse handler requires depth testing to be enabled
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);   // 启用混合
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  text_font_ = new pangolin::GlFont("../config/tt0102m_.ttf", 30.0);
   s_cam_=new pangolin::OpenGlRenderState(
          pangolin::ProjectionMatrix(1480, 760, 840, 840, 480, 380, 0.1, 1000),
          pangolin::ModelViewLookAt(25, 0, 90, 25, 0, 20, pangolin::AxisY)
  );
  const int UI_WIDTH = 30 * pangolin::default_font().MaxWidth();
  d_cam_ = &pangolin::CreateDisplay()
          .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, 1280.0f / 760.0f)
          .SetHandler(new pangolin::Handler3D(*s_cam_));
  pangolin::CreatePanel("ui")
          .SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));

  lvx_checkbox_=new pangolin::Var<bool>("ui.lvx_Checkbox", true, true);
  lvx_file_=new pangolin::Var<std::string> ("ui.lvx_file_String", "../map/bbt/start1.lvx");

  start_button_=new pangolin::Var<bool>("ui.start_Button", false, false);
  stop_button_ =new pangolin::Var<bool>("ui.stop_Button", false, false);
  freq_double_=new pangolin::Var<double>("ui.Freq_Double", 10, 1, 15);

  timequeue_string_=new pangolin::Var<std::string>("ui.tq_String", "timequeue_string");
  xyz_string_=new pangolin::Var<std::string>("ui.xyz_String", "xyz");
  rpy_string_=new pangolin::Var<std::string>("ui.rpy_String", "rpy");
  cost_string_=new pangolin::Var<std::string>("ui.final_cost", "cost");

}

void PangolinViewer::DrawCloud(PointCloud::Ptr cloud,double r,double g,double b,
               double alpha,double psize)
{
  glPointSize(psize);
  glBegin(GL_POINTS);
  glColor4f(r, g, b, alpha);
  for (int i = 0; i < cloud->size(); ++i)
  {
    PointType pt = cloud->points[i];
    glVertex3d(pt.x , pt.y , pt.z );

  }
  glEnd();
}

void PangolinViewer::ShowRealtimePose(Eigen::Matrix4d pose)
{
  double l=2;
  Eigen::Vector3d Ow=pose.topRightCorner(3, 1);
  Eigen::Vector3d Xw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(1,0,0))+Ow;
  Eigen::Vector3d Yw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(0,1,0))+Ow;
  Eigen::Vector3d Zw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(0,0,1))+Ow;

  glLineWidth(5);
  glBegin(GL_LINES);
  glColor3f(1.0, 0.0, 0.0);
  glVertex3d(Ow[0], Ow[1], Ow[2]);
  glVertex3d(Xw[0], Xw[1], Xw[2]);
  glColor3f(0.0, 1.0, 0.0);
  glVertex3d(Ow[0], Ow[1], Ow[2]);
  glVertex3d(Yw[0], Yw[1], Yw[2]);
  glColor3f(0.0, 0.0, 1.0);
  glVertex3d(Ow[0], Ow[1], Ow[2]);
  glVertex3d(Zw[0], Zw[1], Zw[2]);
  glEnd();


  Eigen::Matrix3d rotation = pose.topLeftCorner(3, 3);
  Eigen::Vector3d eulerAngle = rotation.eulerAngles(0, 1, 2);
  Eigen::Vector3d t = pose.topRightCorner(3, 1);
  char rpy[64] = {0};
  sprintf(rpy,
            "%.3f,%.3f,%.3f",
            eulerAngle[0] * 180.0 / M_PI,
            eulerAngle[1] * 180.0 / M_PI,
            eulerAngle[2] * 180.0 / M_PI);
  char translation[64] = {0};
  sprintf(translation, "%.3f,%.3f,%.3f", t[0], t[1], t[2]);
  *xyz_string_ = translation;
  *rpy_string_ = rpy;

}

void PangolinViewer::ShowSlamCost(double cost)
{

    char cost_str[64] = {0};
    sprintf(cost_str, "cost:%f", cost);
    *cost_string_ = cost_str;

}

void PangolinViewer::ShowSlamQueueTime(int queue,double tm)
{
  char info_str[64]={0};
  sprintf(info_str,"time:%.5fs queue:%d",tm,queue);
  *timequeue_string_=info_str;
}

void PangolinViewer::loop(PangolinViewer* p)
{
  if(p== nullptr)
    return ;
  p->Init();
  while (!pangolin::ShouldQuit())
  {
    // Clear entire screen
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    if (pangolin::Pushed(*p->start_button_))
    {
      if (*p->lvx_checkbox_)
      {
        std::string file = p->lvx_file_->Get();
        if(p->startBtn_callback_!= nullptr)
          p->startBtn_callback_(file);
      }
      else
      {
        if(p->startBtn_callback_!= nullptr)
          p->startBtn_callback_("");
      }
    }
    if (pangolin::Pushed(*p->stop_button_))
    {
      if(p->stopBtn_callback_!= nullptr)
        p->stopBtn_callback_();
    }


    if (p->freq_double_->GuiChanged())
    {
      double publish_freq=p->freq_double_->Get();
      if(p->freqChanged_callback_!= nullptr)
        p->freqChanged_callback_(publish_freq);
    }

    if (p->d_cam_->IsShown())
    {
      p->d_cam_->Activate(*p->s_cam_);
      pangolin::glDrawAxis(3);//三维坐标轴，红——x轴，绿——y轴，蓝——z轴
      if(p->spinOnce_callback_!= nullptr)
        p->spinOnce_callback_(p);

    }
    pangolin::FinishFrame();
    std::this_thread::sleep_for(std::chrono::microseconds(10));
  }
}