robotvision/gui_view.cpp at master · t-thanh/robotvision · GitHub

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
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
  /**
 * @author  Hauke Strasdat, Steven Lovegrove
 *
 * Copyright (C) 2010  Hauke Strasdat, Steven Lovegrove,
 *                     Imperial College London
 *
 * gui_view.cpp is part of RobotVision.
 *
 * RobotVision is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation, either version 3 of the
 * License, or any later version.
 *
 * RobotVision is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * and the GNU Lesser General Public License along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 */

#include <GL/gl.h>
#include <GL/glext.h>

#include "gui_view.h"

using namespace CVD;
using namespace RobotVision;
using namespace TooN;
using namespace std;


GuiView::GuiView(const ImageRef & ir,
                 const RobotVision::LinearCamera & cam_pars)
  : AbstractView(ir), cam_pars(cam_pars)
{

}

GuiView::GuiView(const ImageRef & ir,
                 const RobotVision::LinearCamera & cam_pars,
                 const Vector<3>& axis_angle,
                 const Vector<3>& trans)
                   : AbstractView(ir,axis_angle, trans), cam_pars(cam_pars)
{

}

GuiView::GuiView(const GuiView & view)
  : AbstractView(view)
{
}

void GuiView::activate2D()
{
  glShadeModel(GL_FLAT);
  glEnable (GL_LINE_SMOOTH);
  glEnable (GL_BLEND);
  glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  glHint (GL_LINE_SMOOTH_HINT, GL_NICEST);
  glLineWidth (1);

  activate();

  glMatrixMode (GL_PROJECTION);
  glLoadIdentity ();
  ::glOrtho (0, pixel_size[0], pixel_size[1], 0, 0, 1);

  glMatrixMode (GL_MODELVIEW);
  glLoadIdentity();

  glDisable(GL_DEPTH_TEST);
  glDisable(GL_LIGHTING);
}

void GuiView::activate3D()
{
  activate3D(T_cw_);
}

void GuiView::activate3D(const SE3<double> & T_wc)
{
  glShadeModel(GL_FLAT);

  activate();

  glMatrixMode(GL_PROJECTION);

  double fu = cam_pars.F()[0];
  double fv = -cam_pars.F()[1];
  double px = cam_pars.P()[0]+0.5;
  double py = cam_pars.P()[1]+0.5;

  glLoadIdentity();


  GLdouble left, right, bottom, top;
  double nr = 0.1;
  double fr = 1000;

  left = -nr * px / fu;
  top = nr * py / fv;
  right = nr * ( pixel_size[0] - px ) / fu;
  bottom = -nr * ( pixel_size[1] - py ) / fv;

  ::glFrustum( left, right, bottom, top, nr, fr );

  Vector<3> aa = T_wc.get_rotation().ln();
  double angle = norm(aa);

  glScaled(1,1,-1);

  glTranslatef(T_wc.get_translation()[0],T_wc.get_translation()[1],
               T_wc.get_translation()[2]);
  glRotated(angle* 180.0 / M_PI, aa[0], aa[1], aa[2]);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  glEnable(GL_DEPTH_TEST);
  glDisable(GL_LIGHTING);
}