Skip to content

Commit 566b38c

Browse files
GSORFGSORF
committed
Made 3D Visualization more interesting and helpful and changed OutputWrapper to export predicted poses (for testing mapping back in Blender)
1 parent 45bdaa3 commit 566b38c

File tree

5 files changed

+95
-37
lines changed

5 files changed

+95
-37
lines changed

‎03_Application/dso/src/IOWrapper/OutputWrapper/SampleOutputWrapper.h

Lines changed: 10 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -159,15 +159,18 @@ class SampleOutputWrapper : public Output3DWrapper
159159

160160
// First show timestamp (based on 25 fps) in milliseconds:
161161
posesCSV << frame->id * (1.0 / 25.0) * 1000.0 << ",";
162+
163+
// TODO Adam: change back to camToWorld (not predicted), because this is only for testing if mapping works
164+
162165
// Translation:
163-
posesCSV << frame->camToWorld.inverse().translation().row(0) << ","
164-
<< frame->camToWorld.inverse().translation().row(1) << ","
165-
<< frame->camToWorld.inverse().translation().row(2) << ",";
166+
posesCSV << frame->camToWorld_predicted.inverse().translation().row(0) << ","
167+
<< frame->camToWorld_predicted.inverse().translation().row(1) << ","
168+
<< frame->camToWorld_predicted.inverse().translation().row(2) << ",";
166169
// Quaternion:
167-
posesCSV << frame->camToWorld.inverse().unit_quaternion().w() << ","
168-
<< frame->camToWorld.inverse().unit_quaternion().x() << ","
169-
<< frame->camToWorld.inverse().unit_quaternion().y() << ","
170-
<< frame->camToWorld.inverse().unit_quaternion().z()<< "\n";
170+
posesCSV << frame->camToWorld_predicted.inverse().unit_quaternion().w() << ","
171+
<< frame->camToWorld_predicted.inverse().unit_quaternion().x() << ","
172+
<< frame->camToWorld_predicted.inverse().unit_quaternion().y() << ","
173+
<< frame->camToWorld_predicted.inverse().unit_quaternion().z()<< "\n";
171174
posesCSV.flush(); //Flush because Destructor is never called...
172175

173176

‎03_Application/dso/src/IOWrapper/Pangolin/KeyFrameDisplay.cpp

Lines changed: 52 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,7 @@ KeyFrameDisplay::KeyFrameDisplay()
5454
active= true;
5555
camToWorld = SE3();
5656
camToWorld_predicted = SE3(); // ADAM: Added for KalmanDebug
57+
hasPrediction = false; // ADAM: Added for KalmanDebug
5758

5859
needRefresh=true;
5960

@@ -81,6 +82,7 @@ void KeyFrameDisplay::setFromF(FrameShell* frame, CalibHessian* HCalib)
8182
cyi = -cy / fy;
8283
camToWorld = frame->camToWorld;
8384
camToWorld_predicted = frame->camToWorld_predicted; // ADAM: Added for KalmanDebug
85+
hasPrediction = frame->hasPrediction; // ADAM: Added for KalmanDebug
8486
needRefresh=true;
8587
}
8688

@@ -320,7 +322,7 @@ bool KeyFrameDisplay::refreshPC(bool canRefresh, float scaledTH, float absTH, in
320322

321323

322324

323-
void KeyFrameDisplay::drawCam(float lineWidth, float* color, float sizeFactor)
325+
void KeyFrameDisplay::drawCam(float lineWidth, float* color, float sizeFactor, bool drawAxes)
324326
{
325327
if(width == 0)
326328
return;
@@ -365,32 +367,62 @@ void KeyFrameDisplay::drawCam(float lineWidth, float* color, float sizeFactor)
365367
glEnd();
366368

367369
/* ADAM: Added Axis Drawing for Keyframes (maybe leave 1.0 for size?) */
368-
pangolin::glDrawAxis(0.5f);
370+
if(drawAxes) pangolin::glDrawAxis(0.5f);
369371

370372

371373
glPopMatrix();
372374

373375
/* ADAM: Draw Prediction (Kalman filter) */
374-
375-
glPushMatrix();
376-
// Transform from World to (Predicted) Camera
377-
Sophus::Matrix4f m_predicted = camToWorld_predicted.matrix().cast<float>();
378-
glMultMatrixf((GLfloat*)m_predicted.data());
379-
380-
// Draw Coordinate Axes
381-
pangolin::glDrawAxis(0.5f);
382-
383-
// Draw Point for measurement
384-
glBegin(GL_POINTS);
385-
glPointSize(5.0);
386-
glColor3f(1.0f,0.0f,0.0f);
387-
glVertex3f(0.0f, 0.0f, 0.0f);
388-
glEnd();
389-
390-
376+
if(hasPrediction)
377+
{
378+
glPushMatrix();
379+
// Transform from World to (Predicted) Camera
380+
Sophus::Matrix4f m_predicted = camToWorld_predicted.matrix().cast<float>();
381+
glMultMatrixf((GLfloat*)m_predicted.data());
382+
383+
// Draw Coordinate Axes
384+
if(drawAxes) pangolin::glDrawAxis(0.5f);
385+
386+
// Draw Point for measurement
387+
glBegin(GL_POINTS);
388+
glPointSize(5.0);
389+
glColor3f(1.0f,0.0f,0.0f);
390+
glVertex3f(0.0f, 0.0f, 0.0f);
391+
glEnd();
392+
393+
glBegin(GL_LINES);
394+
// Draw Camera for visualization
395+
glVertex3f(0,0,0);
396+
glVertex3f(sz*(0-cx)/fx,sz*(0-cy)/fy,sz);
397+
glVertex3f(0,0,0);
398+
glVertex3f(sz*(0-cx)/fx,sz*(height-1-cy)/fy,sz);
399+
glVertex3f(0,0,0);
400+
glVertex3f(sz*(width-1-cx)/fx,sz*(height-1-cy)/fy,sz);
401+
glVertex3f(0,0,0);
402+
glVertex3f(sz*(width-1-cx)/fx,sz*(0-cy)/fy,sz);
403+
404+
glVertex3f(sz*(width-1-cx)/fx,sz*(0-cy)/fy,sz);
405+
glVertex3f(sz*(width-1-cx)/fx,sz*(height-1-cy)/fy,sz);
391406

407+
glVertex3f(sz*(width-1-cx)/fx,sz*(height-1-cy)/fy,sz);
408+
glVertex3f(sz*(0-cx)/fx,sz*(height-1-cy)/fy,sz);
409+
410+
glVertex3f(sz*(0-cx)/fx,sz*(height-1-cy)/fy,sz);
411+
glVertex3f(sz*(0-cx)/fx,sz*(0-cy)/fy,sz);
412+
413+
glVertex3f(sz*(0-cx)/fx,sz*(0-cy)/fy,sz);
414+
glVertex3f(sz*(width-1-cx)/fx,sz*(0-cy)/fy,sz);
415+
416+
glEnd();
417+
418+
419+
420+
421+
glPopMatrix();
422+
423+
}
392424

393-
glPopMatrix();
425+
/*END ADAM: Draw Prediction (Kalman filter) */
394426

395427

396428
}

‎03_Application/dso/src/IOWrapper/Pangolin/KeyFrameDisplay.h

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -81,13 +81,14 @@ class KeyFrameDisplay
8181
bool refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity);
8282

8383
// renders cam & pointcloud.
84-
void drawCam(float lineWidth = 1, float* color = 0, float sizeFactor=1);
84+
void drawCam(float lineWidth = 1, float* color = 0, float sizeFactor=1, bool drawAxes=false);
8585
void drawPC(float pointSize);
8686

8787
int id;
8888
bool active;
8989
SE3 camToWorld;
90-
SE3 camToWorld_predicted; // ADAM: Changed to display kalman Filter
90+
SE3 camToWorld_predicted; // ADAM: Added to display kalman Filter
91+
bool hasPrediction; // ADAM: Added to display kalman Filter
9192

9293
inline bool operator < (const KeyFrameDisplay& other) const
9394
{

‎03_Application/dso/src/IOWrapper/Pangolin/PangolinDSOViewer.cpp

Lines changed: 27 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -132,6 +132,8 @@ void PangolinDSOViewer::run()
132132
pangolin::Var<bool> settings_showFullTrajectory("ui.FullTrajectory",false,true);
133133
pangolin::Var<bool> settings_showActiveConstraints("ui.ActiveConst",true,true);
134134
pangolin::Var<bool> settings_showAllConstraints("ui.AllConst",false,true);
135+
pangolin::Var<bool> settings_showWorldGrid("ui.showWorldGrid",true,true);
136+
pangolin::Var<bool> settings_showAxes("ui.showCoordinateSystems",true,true);
135137

136138

137139
pangolin::Var<bool> settings_show3D("ui.show3D",true,true);
@@ -177,23 +179,42 @@ void PangolinDSOViewer::run()
177179

178180
//ADAM: Changed (commented back in):
179181
//pangolin::glDrawColouredCube();
180-
glLineWidth(lineWidth*2.0f);
181-
pangolin::glDrawAxis(1.0f);
182+
float lineWidth = 1.0f;
183+
glLineWidth(lineWidth*4.0f);
184+
pangolin::glDrawAxis(5.0f);
182185

186+
//DrawGrid:
187+
if(this->settings_showWorldGrid)
188+
{
189+
glLineWidth(lineWidth);
190+
int gridSize = 10;
191+
glBegin(GL_LINES);
192+
for(int x = -gridSize; x <= gridSize; x++)
193+
{
194+
glVertex2f(x, -gridSize);
195+
glVertex2f(x, gridSize);
196+
}
197+
for(int y = -gridSize; y <= gridSize; y++)
198+
{
199+
glVertex2f(-gridSize, y);
200+
glVertex2f(gridSize, y);
201+
}
202+
glEnd();
203+
}
183204
//END ADAM
184205

185206
int refreshed=0;
186207
for(KeyFrameDisplay* fh : keyframes)
187208
{
188209
float blue[3] = {0,0,1};
189-
if(this->settings_showKFCameras) fh->drawCam(1,blue,0.1);
210+
if(this->settings_showKFCameras) fh->drawCam(1,blue,0.1, this->settings_showAxes);
190211

191212

192213
refreshed =+ (int)(fh->refreshPC(refreshed < 10, this->settings_scaledVarTH, this->settings_absVarTH,
193214
this->settings_pointCloudMode, this->settings_minRelBS, this->settings_sparsity));
194215
fh->drawPC(1);
195216
}
196-
if(this->settings_showCurrentCamera) currentCam->drawCam(2,0,0.2);
217+
if(this->settings_showCurrentCamera) currentCam->drawCam(2,0,0.2, this->settings_showAxes);
197218
drawConstraints();
198219
lk3d.unlock();
199220
}
@@ -258,6 +279,8 @@ void PangolinDSOViewer::run()
258279
this->settings_showKFCameras = settings_showKFCameras.Get();
259280
this->settings_showTrajectory = settings_showTrajectory.Get();
260281
this->settings_showFullTrajectory = settings_showFullTrajectory.Get();
282+
this->settings_showWorldGrid = settings_showWorldGrid.Get();
283+
this->settings_showAxes = settings_showAxes.Get();
261284

262285
setting_render_display3D = settings_show3D.Get();
263286
setting_render_displayDepth = settings_showLiveDepth.Get();
@@ -422,10 +445,6 @@ void PangolinDSOViewer::drawConstraints()
422445
}
423446

424447

425-
426-
427-
428-
429448
void PangolinDSOViewer::publishGraph(const std::map<uint64_t,Eigen::Vector2i> &connectivity)
430449
{
431450
if(!setting_render_display3D) return;

‎03_Application/dso/src/IOWrapper/Pangolin/PangolinDSOViewer.h

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -117,6 +117,9 @@ class PangolinDSOViewer : public Output3DWrapper
117117
bool settings_showFullTrajectory;
118118
bool settings_showActiveConstraints;
119119
bool settings_showAllConstraints;
120+
/* ADAM: UI controls to show worldGrid and Coordinate Systems */
121+
bool settings_showWorldGrid;
122+
bool settings_showAxes;
120123

121124
float settings_scaledVarTH;
122125
float settings_absVarTH;

0 commit comments

Comments
 (0)