Skip to content

Commit abb6c3a

Browse files
author
robotvision
committed
RELEASE 1.1.0
- faster BA implementation! - many small improvements/fixes
1 parent 3e55e96 commit abb6c3a

27 files changed

+2420
-1581
lines changed

‎CMakeLists.txt

+2-2
Original file line numberDiff line numberDiff line change
@@ -40,9 +40,9 @@ FOREACH(lib ${LIB_NAMES})
4040
LIST(APPEND LIBS ${LIB_${lib}})
4141
ENDFOREACH(lib)
4242

43-
SET (CLASSES gui_view abstract_view gui_window maths_utils image Camera/linear_camera opencv_helper )
43+
SET (CLASSES gui_view abstract_view gui_window maths_utils image Camera/linear_camera opencv_helper sparse_matrix )
4444

45-
SET (SOURCES quadtree.h stopwatch.h bundle_adjuster.h graph_optimizer.h transformations.h sparse_cholesky.h sparse_matrix.h rectangle.h sim3.h Camera/abstract_camera.h )
45+
SET (SOURCES quadtree.h stopwatch.h bundle_adjuster.h graph_optimizer.h transformations.h sparse_cholesky.h rectangle.h sim3.h Camera/abstract_camera.h )
4646

4747

4848
# Append CSparse Sources

‎Camera/abstract_camera.h

+10-4
Original file line numberDiff line numberDiff line change
@@ -52,10 +52,16 @@ namespace RobotVision {
5252
virtual TooN::Vector<2> unmap(const TooN::Vector<2>& imframe) const = 0;
5353

5454

55-
virtual TooN::Matrix<2>
56-
jacobian(const TooN::Vector<2>& camframe) const = 0;
57-
virtual TooN::Matrix<2>
58-
inv_jacobian(const TooN::Vector<2>& imframe) const = 0;
55+
virtual TooN::Matrix<2> jacobian(const TooN::Vector<2>& camframe) const = 0;
56+
57+
58+
virtual const TooN::Vector<> params() const = 0;
59+
60+
// Project p_cam in to the camera, applying intrinsics and distortion
61+
inline TooN::Vector<2> projectmap(const TooN::Vector<3>& p_cam) const
62+
{
63+
return map( TooN::project(p_cam) );
64+
}
5965

6066
inline int width() const { return size().x; }
6167
inline int height() const { return size().y; }

‎Camera/linear_camera.cpp

+44-1
Original file line numberDiff line numberDiff line change
@@ -30,15 +30,57 @@ using namespace TooN;
3030

3131
namespace RobotVision{
3232

33+
LinearCamera::LinearCamera()
34+
: fl_and_pp(4),
35+
focal_length(makeVector(1,1)),
36+
principle_point(Zeros),
37+
image_size(-1,-1) ,
38+
intrinsics(Identity),
39+
inv_intrinsics(Identity(3))
40+
{
41+
fl_and_pp.slice<0,2>() = focal_length;
42+
fl_and_pp.slice<2,2>() = principle_point;
43+
}
44+
45+
LinearCamera::LinearCamera(const AbstractCamera& cam)
46+
:fl_and_pp(4),
47+
focal_length(cam.F()),
48+
principle_point(cam.P()),
49+
image_size(cam.size()) ,
50+
intrinsics(Identity),
51+
inv_intrinsics(Identity(3))
52+
{
53+
initialise();
54+
}
55+
56+
LinearCamera::LinearCamera(const TooN::Matrix<3,3> K,
57+
const CVD::ImageRef & size)
58+
:fl_and_pp(4),
59+
focal_length(makeVector( K[0][0]/K[2][2], K[1][1]/K[2][2])),
60+
principle_point(makeVector( K[0][2]/K[2][2], K[1][2]/K[2][2])),
61+
image_size(size),
62+
intrinsics(Identity)
63+
{
64+
initialise();
65+
}
66+
3367
LinearCamera::LinearCamera(const Vector<2> & focal_length,
3468
const Vector<2> & principle_point,
3569
const CVD::ImageRef & size)
36-
: focal_length(focal_length) ,
70+
:fl_and_pp(4),
71+
focal_length(focal_length),
3772
principle_point(principle_point),
3873
image_size(size) ,
3974
intrinsics(Identity),
4075
inv_intrinsics(Identity(3))
4176
{
77+
initialise();
78+
}
79+
80+
void LinearCamera::initialise()
81+
{
82+
fl_and_pp.slice<0,2>() = focal_length;
83+
fl_and_pp.slice<2,2>() = principle_point;
4284
inv_focal_length = makeVector(1./focal_length[0], 1./focal_length[1]);
4385
intrinsics(0,0) = focal_length[0];
4486
intrinsics(1,1) = focal_length[1];
@@ -50,6 +92,7 @@ namespace RobotVision{
5092
inv_intrinsics(1,2) = -principle_point[1]*inv_focal_length[1];
5193
}
5294

95+
5396
TooN::Vector<2> LinearCamera::map(const TooN::Vector<2>& point) const
5497
{
5598
return element_product(focal_length,point) + principle_point;

‎Camera/linear_camera.h

+26-1
Original file line numberDiff line numberDiff line change
@@ -31,8 +31,19 @@ namespace RobotVision {
3131
class LinearCamera : public AbstractCamera
3232
{
3333
public:
34-
LinearCamera() {}
34+
static const int NUM_PARAMS = 4;
3535

36+
// Construct Identity Camera
37+
LinearCamera();
38+
39+
// Contract from Linear portion of cam
40+
LinearCamera(const AbstractCamera& cam);
41+
42+
// Construct Linear camera given intrinsics matrix
43+
LinearCamera(const TooN::Matrix<3,3> K,
44+
const CVD::ImageRef & size);
45+
46+
// Construct Linear camera given parameters
3647
LinearCamera(const TooN::Vector<2> & focal_length,
3748
const TooN::Vector<2> & principle_point,
3849
const CVD::ImageRef & size);
@@ -54,6 +65,12 @@ namespace RobotVision {
5465
return focal_length;
5566
}
5667

68+
/// Focal Length
69+
const TooN::Vector<2>& Finv() const
70+
{
71+
return inv_focal_length;
72+
}
73+
5774
const TooN::Matrix<3,3>& K() const
5875
{
5976
return intrinsics;
@@ -64,6 +81,11 @@ namespace RobotVision {
6481
return inv_intrinsics;
6582
}
6683

84+
const TooN::Vector<> params() const
85+
{
86+
return fl_and_pp;
87+
}
88+
6789
TooN::Vector<2> map(const TooN::Vector<2>& camframe) const;
6890
TooN::Vector<2> unmap(const TooN::Vector<2>& imframe) const;
6991

@@ -74,7 +96,10 @@ namespace RobotVision {
7496
TooN::Matrix<2> inv_jacobian() const;
7597

7698
private:
99+
void initialise();
100+
77101
//Trick: Do computation once, and store ambiguous represenations
102+
TooN::Vector<> fl_and_pp; //
78103
TooN::Vector<2> focal_length;
79104
TooN::Vector<2> principle_point;
80105
CVD::ImageRef image_size;

‎INSTALL.txt

+2-2
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,9 @@ is already checked out using the following command:
66

77
> svn co https://svn.openslam.org/data/svn/robotvision
88

9-
First, make sure that CMake is installed:
9+
First, make sure that CMake and OpenGL is installed:
1010

11-
> sudo apt-get install cmake
11+
> sudo apt-get install cmake libglu1-mesa-dev
1212

1313
Then, install TooN. Make sure that you install the right version (10 April 2010
1414
version).

‎VERSION.txt

+1-1
Original file line numberDiff line numberDiff line change
@@ -1 +1 @@
1-
1.0.2
1+
1.1.0

‎abstract_view.cpp

+45-17
Original file line numberDiff line numberDiff line change
@@ -59,7 +59,7 @@ TooN::SE3<> AbstractView::getPose()
5959
}
6060

6161
AbstractView::AbstractView(const ImageRef & size)
62-
: handler(NULL), pixel_size( size ), T_cw_(Identity(3), makeVector(0,0,3))
62+
: Viewport(size), handler(NULL), T_cw_(Identity(3), makeVector(0,0,3))
6363

6464
{
6565
qtree = new QuadTree<int>(RobotVision::Rectangle(0,0,size.x,size.y),1);
@@ -68,19 +68,18 @@ AbstractView::AbstractView(const ImageRef & size)
6868
AbstractView::AbstractView(const ImageRef & size,
6969
const Vector<3>& axis_angle,
7070
const Vector<3>& trans)
71-
: handler(NULL),
72-
pixel_size( size ),
71+
: Viewport(size),handler(NULL),
7372
T_cw_(SO3<>(axis_angle),trans)
7473
{
7574
qtree = new QuadTree<int>(RobotVision::Rectangle(0,0,size.x,size.y),1);
7675
}
7776

7877
AbstractView::AbstractView(const AbstractView & view)
79-
: handler(NULL), pixel_size( view.pixel_size )
78+
: Viewport(view.pixel_size), handler(NULL)
8079
{
8180
qtree
8281
= new QuadTree<int>(Rectangle(0,0,
83-
view.pixel_size.x,view.pixel_size.y),1);
82+
view.pixel_size[0],view.pixel_size[1]),1);
8483
}
8584

8685
CVD::GLWindow::EventHandler* AbstractView::get_handler()
@@ -101,7 +100,7 @@ GuiWindow* AbstractView::parent()
101100

102101
bool AbstractView::isNavigating()
103102
{
104-
return win && win->active_view == this;
103+
return win && win->get_active_view() == this;
105104
}
106105

107106
TooN::Vector<3> AbstractView::intersectScene( const CVD::ImageRef& ip )
@@ -125,8 +124,8 @@ TooN::Vector<3> AbstractView::intersectScene( const CVD::ImageRef& ip )
125124
glK[0][3], glK[1][3], glK[2][3], glK[3][3]
126125
};
127126

128-
const GLfloat winx = viewport[0]+((float)ip.x / pixel_size.x)*viewport[2];
129-
const GLfloat winy = viewport[1]+((float)ip.y / pixel_size.y)*viewport[3];
127+
const GLfloat winx = viewport[0]+((float)ip.x / pixel_size[0])*viewport[2];
128+
const GLfloat winy = viewport[1]+((float)ip.y / pixel_size[1])*viewport[3];
130129
GLfloat winz;
131130
glReadPixels((GLint)winx,(GLint)winy,1,1,GL_DEPTH_COMPONENT,GL_FLOAT,&winz);
132131

@@ -140,6 +139,36 @@ TooN::Vector<3> AbstractView::intersectScene( const CVD::ImageRef& ip )
140139
}
141140
}
142141

142+
void AbstractView::drawTexture2D(const SubImage<float> & img)
143+
{
144+
glEnable(GL_TEXTURE_2D);
145+
GLuint texture;
146+
glGenTextures(1, &texture);
147+
glBindTexture(GL_TEXTURE_2D, texture);
148+
glTexImage2D(img);
149+
150+
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST);
151+
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST);
152+
glBindTexture(GL_TEXTURE_2D, texture);
153+
154+
float x1 = 0;
155+
float y1 = 0;
156+
float x2 = pixel_size[0];
157+
float y2 = pixel_size[1];
158+
159+
glBegin(GL_QUADS);
160+
161+
glTexCoord2f(0.0f, 0.0f); glVertex2i(0, 0);
162+
glTexCoord2f(0.0f, 1.0f); glVertex2i(0, y2-y1);
163+
glTexCoord2f(1.0f, 1.0f); glVertex2i(x2-x1, y2-y1);
164+
glTexCoord2f(1.0f, 0.0f); glVertex2i(x2-x1, 0);
165+
166+
glEnd();
167+
168+
glDeleteTextures(1,&texture);
169+
glDisable(GL_TEXTURE_2D);
170+
}
171+
143172
void AbstractView::drawTexture2D(const SubImage<CVD::byte> & img)
144173
{
145174
glEnable(GL_TEXTURE_2D);
@@ -154,8 +183,8 @@ void AbstractView::drawTexture2D(const SubImage<CVD::byte> & img)
154183

155184
float x1 = 0;
156185
float y1 = 0;
157-
float x2 = pixel_size.x;
158-
float y2 = pixel_size.y;
186+
float x2 = pixel_size[0];
187+
float y2 = pixel_size[1];
159188

160189
glBegin(GL_QUADS);
161190

@@ -284,8 +313,8 @@ void AbstractView::drawTexture2D(const SubImage<Rgb<CVD::byte> > & img)
284313

285314
float x1 = 0;
286315
float y1 = 0;
287-
float x2 = pixel_size.x;
288-
float y2 = pixel_size.y;
316+
float x2 = pixel_size[0];
317+
float y2 = pixel_size[1];
289318

290319
glBegin(GL_QUADS);
291320

@@ -470,23 +499,22 @@ void AbstractView::activate()
470499
}
471500

472501
void AbstractView::init(GuiWindow * win, const RobotVision::Rectangle & box){
473-
this->win = win;
474502
win->connectView(*this,box);
475503
}
476504

477-
478505
void AbstractView::drawCovariance3D(const Vector<3> & trans,
479506
const Matrix<3> & pose_unc,
480507
double number_of_sigma){
481508
//ToDo: Check whether this is doing the right thing!
509+
//Should be alright now!
482510
glPushMatrix();
483511
glTranslatef(trans[0], trans[1], trans[2]);
484512

485513
SymEigen<3> e_system(pose_unc);
486514
Matrix<3> & V = e_system.get_evectors();
487-
GLfloat Varray[ 16 ] = {V( 0, 0 ), V( 1, 0 ), V( 2, 0 ), 0.0,
488-
V( 0, 1 ), V( 1, 1 ), V( 2, 1 ), 0.0,
489-
V( 0, 2 ), V( 1, 2 ), V( 2, 2 ), 0.0,
515+
GLfloat Varray[ 16 ] = {V( 0, 0 ), V( 0, 1 ), V( 0, 2 ), 0.0,
516+
V( 1, 0 ), V( 1, 1 ), V( 1, 2 ), 0.0,
517+
V( 2, 0 ), V( 2, 1 ), V( 2, 2 ), 0.0,
490518
0.0, 0.0, 0.0, 1.0};
491519
glMultMatrixf( Varray );
492520
Vector<3> v = e_system.get_evalues();

‎abstract_view.h

+5-8
Original file line numberDiff line numberDiff line change
@@ -40,9 +40,8 @@ namespace RobotVision
4040
{
4141
class GuiWindow;
4242

43-
class AbstractView
43+
class AbstractView : public Viewport
4444
{
45-
friend class GuiWindow;
4645
public:
4746
AbstractView()
4847
{}
@@ -85,6 +84,7 @@ namespace RobotVision
8584
virtual void activate3D(const TooN::SE3<double> & pose) = 0;
8685

8786
void drawTexture2D(const CVD::SubImage<CVD::byte> & img);
87+
void drawTexture2D(const CVD::SubImage<float> & img);
8888
void drawTexture2D(const CVD::SubImage<CVD::Rgb<CVD::byte> > & img);
8989
void drawTexture2D(const CVD::SubImage<CVD::byte> & img,
9090
const CVD::ImageRef & ir);
@@ -125,7 +125,7 @@ namespace RobotVision
125125
TooN::SE3<> getPose();
126126

127127
inline CVD::ImageRef size() const {
128-
return pixel_size;
128+
return CVD::ImageRef(pixel_size[0],pixel_size[1]);
129129
}
130130

131131
inline CVD::ImageRef viewSize() const {
@@ -135,21 +135,18 @@ namespace RobotVision
135135

136136
protected:
137137
CVD::GLWindow::EventHandler* handler;
138-
CVD::ImageRef pixel_size;
139138
TooN::Matrix<4> glK;
140139
TooN::SE3<double> T_cw_;
141140
TooN::Vector<2> pixel2opengl(const TooN::Vector<2> & v);
142-
GuiWindow * win;
143141
int id;
144-
Rectangle bounding_box;
145142
TooN::QuadTree<int> * qtree;
146143
};
147144

148145
class DoomViewHandler : public CVD::GLWindow::EventHandler
149146
{
150147
public:
151-
DoomViewHandler( AbstractView* view ) : view(view), enabled(false) {}
152-
void on_key_down(CVD::GLWindow& /*win*/, int /*key*/);
148+
DoomViewHandler( AbstractView* view ) : view(view), enabled(true) {}
149+
virtual void on_key_down(CVD::GLWindow& /*win*/, int /*key*/);
153150
void on_mouse_down(CVD::GLWindow& /*win*/,
154151
CVD::ImageRef /*where*/,
155152
int /*state*/,

0 commit comments

Comments
 (0)