c++ Eigen Pangolin（五）

参考：

1. 视觉SLAM十四讲link

摘自：

1. visualizeGeometry

cmakelist.txt

cmake_minimum_required(VERSION 2.8)
project(chapter3)

set(CMAKE_CXX_FLAGS "-std=c++14")

include_directories("/usr/include/eigen3")
find_package(Pangolin REQUIRED)


add_executable(visualizeGeometry1 visualizeGeometry1.cpp)
target_link_libraries(visualizeGeometry1 ${Pangolin_LIBRARIES}
        /usr/local/lib/libpango_core.so)

visualizeGeometry1.cpp

#include <iostream>
#include <iomanip>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <pangolin/pangolin.h>


struct RotationMatrix{
    Eigen::Matrix3f matrix = Eigen::Matrix3f::Identity();
};

struct TranslationVector{
    Eigen::Vector3f trans = Eigen::Vector3f(0, 0, 0);
};

struct QuaternionDraw {
    Eigen::Quaterniond q;
};

std::ostream & operator<<(std::ostream &out, const RotationMatrix &r){
    out.setf(std::ios::fixed);
    Eigen::Matrix3f matrix = r.matrix;
    out << '=';
    out << "[" << std::setprecision(3)
               << matrix(0, 0) << "," << matrix(0, 1) << "," << matrix(0, 2) << "],"
        << "[" << matrix(1, 0) << "," << matrix(1, 1) << "," << matrix(1, 2) << "],"
        << "[" << matrix(2, 0) << "," << matrix(2, 1) << "," << matrix(2, 2) << "]";
    return out;
}

std::istream &operator>>(std::istream &in, RotationMatrix &r){
    return in;
}

std::ostream &operator<<(std::ostream &out, const TranslationVector &t) {
    out << "=[" << t.trans(0) << ',' << t.trans(1) << ',' << t.trans(2) << "]";
    return out;
}

std::istream &operator>>(std::istream &in, TranslationVector &t) {
    return in;
}


std::ostream &operator<<(std::ostream &out, const QuaternionDraw quat) {
    auto c = quat.q.coeffs();
    out << "=[" << c[0] << "," << c[1] << "," << c[2] << "," << c[3] << "]";
    return out;
}

std::istream &operator>>(std::istream &in, const QuaternionDraw quat) {
    return in;
}

int main(int argc, char **argv){

    pangolin::CreateWindowAndBind("visualize geometry", 1280,720);
    glEnable(GL_DEPTH_TEST);
    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1000, 600, 420, 420, 500, 300, 0.1, 1000),
            pangolin::ModelViewLookAt(3, 3, 3, 0, 0, 0, pangolin::AxisY)
    );

    const int UI_WIDTH = 500;
    pangolin::View &d_cam = pangolin::CreateDisplay().
            SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f / 600.0f).
            SetHandler(new pangolin::Handler3D(s_cam));

    //UI
    pangolin::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix());
    pangolin::Var<TranslationVector> translation_vector("ui.t",TranslationVector());
    pangolin::Var<TranslationVector> euler_angles("ui.rpy", TranslationVector());
    pangolin::Var<QuaternionDraw> quaternion("ui.q", QuaternionDraw());
    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));

    while(!pangolin::ShouldQuit()){

        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);

        pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();
        Eigen::Matrix<float, 4, 4> m = matrix;
        RotationMatrix R;
        for(int i = 0; i < 3; i++){
            for(int j = 0; j < 3; j++){
                R.matrix(i, j) = m(j, i);
            }
        }
        rotation_matrix = R;

        TranslationVector t;
        t.trans = Eigen::Vector3f(m(0, 3), m(1, 3), m(2, 3));
        t.trans = -R.matrix * t.trans;
        translation_vector = t;

        TranslationVector euler;
        euler.trans = R.matrix.eulerAngles(2, 1, 0);
        euler_angles = euler;

        QuaternionDraw quat;
        quat.q = Eigen::Quaterniond(R.matrix.cast<double>());
        quaternion = quat;

        glColor3f(1.0, 1.0, 1.0);

        pangolin::glDrawColouredCube();
        // draw the original axis
        glLineWidth(3);
        glColor3f(0.8f, 0.f, 0.f);
        glBegin(GL_LINES);
        glVertex3f(0, 0, 0);
        glVertex3f(10, 0, 0);

        glColor3f(0.f, 0.8f, 0.f);
        glVertex3f(0, 0, 0);
        glVertex3f(0, 10, 0);

        glColor3f(0.2f, 0.2f, 1.f);
        glVertex3f(0, 0, 0);
        glVertex3f(0, 0, 10);
        glEnd();

        pangolin::FinishFrame();
	}
	
	return 0;
}