-
Notifications
You must be signed in to change notification settings - Fork 4
/
se3_plot.hpp
39 lines (33 loc) · 1.15 KB
/
se3_plot.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
#pragma once
#include<iostream>
#include"se3.hpp"
namespace liespline {
template<typename Out_stream>
void plot_se3(const Isometryd3& transform, Out_stream&& out_stream, const double size = 1.)
{
auto origin = transform * Eigen::Vector3d::Zero();
auto unit_x = transform * (Eigen::Vector3d::UnitX() * size);
auto unit_y = transform * (Eigen::Vector3d::UnitY() * size);
auto unit_z = transform * (Eigen::Vector3d::UnitZ() * size);
out_stream << origin.transpose() << " 0xFF0000" << std::endl
<< unit_x.transpose() << " 0xFF0000" << std::endl
<< std::endl
<< std::endl;
out_stream << origin.transpose() << " 0x00FF00" << std::endl
<< unit_y.transpose() << " 0x00FF00" << std::endl
<< std::endl
<< std::endl;
out_stream << origin.transpose() << " 0x0000FF" << std::endl
<< unit_z.transpose() << " 0x0000FF" << std::endl
<< std::endl
<< std::endl;
}
/*
set view equal xyz
set view ,,1.0
set xlabel "x"
set ylabel "y"
set zlabel "z"
splot "<./a.out" with lines lc rgb variable
*/
}