c++(4) sophus可视化和计算误差

标签：p2 p1 1.0 TrajectoryType pangolin c++ 计算误差 sophus translation

 

 

 

 

 

 

 

 

 CMakeLists.txt

project(test)


find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})

find_package(fmt REQUIRED)
set(FMT_LIBRARIES fmt::fmt)

#set(v1_node_file main.cpp)

add_executable(v1_node main.cpp)
target_link_libraries(v1_node ${Pangolin_LIBRARIES} ${FMT_LIBRARIES})

　　main.cpp

#ifndef MAIN_CPP
#define MAIN_CPP


#include "API_draw.h"


string groundtruth_file = "./data/groundtruth.txt";
string estimated_file = "./data/estimated.txt";



int main(int argc, char **argv) {
  TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
  TrajectoryType estimated = ReadTrajectory(estimated_file);
  assert(!groundtruth.empty() && !estimated.empty());
  assert(groundtruth.size() == estimated.size());

  // compute rmse
  double rmse = 0;
  for (size_t i = 0; i < estimated.size(); i++) {
    Sophus::SE3d p1 = estimated[i], p2 = groundtruth[i];
    double error = (p2.inverse() * p1).log().norm();
    rmse += error * error;
  }
  rmse = rmse / double(estimated.size());
  rmse = sqrt(rmse);
  cout << "RMSE = " << rmse << endl;

  DrawTrajectory(groundtruth, estimated);
  return 0;
}




#endif 

　　API_draw.h

 

#ifndef DRAW_CPP
#define DRAW_CPP




#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>

using namespace Sophus;
using namespace std;



typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti);

TrajectoryType ReadTrajectory(const string &path);

TrajectoryType ReadTrajectory(const string &path) {
  ifstream fin(path);
  TrajectoryType trajectory;
  if (!fin) {
    cerr << "trajectory " << path << " not found." << endl;
    return trajectory;
  }

  while (!fin.eof()) {
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
    trajectory.push_back(p1);
  }
  return trajectory;
}

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti) {
  // 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() == false) {
    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);
    for (size_t i = 0; i < gt.size() - 1; i++) {
      glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truth
      glBegin(GL_LINES);
      auto p1 = gt[i], p2 = gt[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }

    for (size_t i = 0; i < esti.size() - 1; i++) {
      glColor3f(1.0f, 0.0f, 0.0f);  // red for estimated
      glBegin(GL_LINES);
      auto p1 = esti[i], p2 = esti[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }

}


#endif 

　　

#ifndef DRAW_CPP #defineDRAW_CPP



#include<iostream> #include<fstream> #include<unistd.h> #include<pangolin/pangolin.h> #include<sophus/se3.hpp>
using namespace Sophus; using namespace std;


typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti);
TrajectoryType ReadTrajectory(const string &path);
TrajectoryType ReadTrajectory(const string &path) { ifstream fin(path); TrajectoryType trajectory; if (!fin) { cerr << "trajectory " << path << " not found." << endl; return trajectory; }
while (!fin.eof()) { double time, tx, ty, tz, qx, qy, qz, qw; fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw; Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz)); trajectory.push_back(p1); } return trajectory; }
void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti) { // 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() == false) { 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); for (size_t i = 0; i < gt.size() - 1; i++) { glColor3f(0.0f, 0.0f, 1.0f); // blue for ground truth glBegin(GL_LINES); auto p1 = gt[i], p2 = gt[i + 1]; glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]); glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]); glEnd(); }
for (size_t i = 0; i < esti.size() - 1; i++) { glColor3f(1.0f, 0.0f, 0.0f); // red for estimated glBegin(GL_LINES); auto p1 = esti[i], p2 = esti[i + 1]; glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]); glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]); glEnd(); } pangolin::FinishFrame(); usleep(5000); // sleep 5 ms }
}

#endif  

标签：p2,p1,1.0,TrajectoryType,pangolin,c++,计算误差,sophus,translation
From： https://www.cnblogs.com/gooutlook/p/18317025