Skip to content

Commit

Permalink
Made 3D Visualization more interesting and helpful and changed Output…
Browse files Browse the repository at this point in the history 
…Wrapper to export predicted poses (for testing mapping back in Blender)
  • Loading branch information
@GSORF
GSORF committed Apr 11, 2018
1 parent 45bdaa3 commit 566b38c
Show file tree
Hide file tree
Showing 5 changed files with 95 additions and 37 deletions.
17 changes: 10 additions & 7 deletions 03_Application/dso/src/IOWrapper/OutputWrapper/SampleOutputWrapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -159,15 +159,18 @@ class SampleOutputWrapper : public Output3DWrapper

// First show timestamp (based on 25 fps) in milliseconds:
posesCSV << frame->id * (1.0 / 25.0) * 1000.0 << ",";

// TODO Adam: change back to camToWorld (not predicted), because this is only for testing if mapping works

// Translation:
posesCSV << frame->camToWorld.inverse().translation().row(0) << ","
<< frame->camToWorld.inverse().translation().row(1) << ","
<< frame->camToWorld.inverse().translation().row(2) << ",";
posesCSV << frame->camToWorld_predicted.inverse().translation().row(0) << ","
<< frame->camToWorld_predicted.inverse().translation().row(1) << ","
<< frame->camToWorld_predicted.inverse().translation().row(2) << ",";
// Quaternion:
posesCSV << frame->camToWorld.inverse().unit_quaternion().w() << ","
<< frame->camToWorld.inverse().unit_quaternion().x() << ","
<< frame->camToWorld.inverse().unit_quaternion().y() << ","
<< frame->camToWorld.inverse().unit_quaternion().z()<< "\n";
posesCSV << frame->camToWorld_predicted.inverse().unit_quaternion().w() << ","
<< frame->camToWorld_predicted.inverse().unit_quaternion().x() << ","
<< frame->camToWorld_predicted.inverse().unit_quaternion().y() << ","
<< frame->camToWorld_predicted.inverse().unit_quaternion().z()<< "\n";
posesCSV.flush(); //Flush because Destructor is never called...


Expand Down
72 changes: 52 additions & 20 deletions 03_Application/dso/src/IOWrapper/Pangolin/KeyFrameDisplay.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ KeyFrameDisplay::KeyFrameDisplay()
active= true;
camToWorld = SE3();
camToWorld_predicted = SE3(); // ADAM: Added for KalmanDebug
hasPrediction = false; // ADAM: Added for KalmanDebug

needRefresh=true;

Expand Down Expand Up @@ -81,6 +82,7 @@ void KeyFrameDisplay::setFromF(FrameShell* frame, CalibHessian* HCalib)
cyi = -cy / fy;
camToWorld = frame->camToWorld;
camToWorld_predicted = frame->camToWorld_predicted; // ADAM: Added for KalmanDebug
hasPrediction = frame->hasPrediction; // ADAM: Added for KalmanDebug
needRefresh=true;
}

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



void KeyFrameDisplay::drawCam(float lineWidth, float* color, float sizeFactor)
void KeyFrameDisplay::drawCam(float lineWidth, float* color, float sizeFactor, bool drawAxes)
{
if(width == 0)
return;
Expand Down Expand Up @@ -365,32 +367,62 @@ void KeyFrameDisplay::drawCam(float lineWidth, float* color, float sizeFactor)
glEnd();

/* ADAM: Added Axis Drawing for Keyframes (maybe leave 1.0 for size?) */
pangolin::glDrawAxis(0.5f);
if(drawAxes) pangolin::glDrawAxis(0.5f);


glPopMatrix();

/* ADAM: Draw Prediction (Kalman filter) */

glPushMatrix();
// Transform from World to (Predicted) Camera
Sophus::Matrix4f m_predicted = camToWorld_predicted.matrix().cast<float>();
glMultMatrixf((GLfloat*)m_predicted.data());

// Draw Coordinate Axes
pangolin::glDrawAxis(0.5f);

// Draw Point for measurement
glBegin(GL_POINTS);
glPointSize(5.0);
glColor3f(1.0f,0.0f,0.0f);
glVertex3f(0.0f, 0.0f, 0.0f);
glEnd();


if(hasPrediction)
{
glPushMatrix();
// Transform from World to (Predicted) Camera
Sophus::Matrix4f m_predicted = camToWorld_predicted.matrix().cast<float>();
glMultMatrixf((GLfloat*)m_predicted.data());

// Draw Coordinate Axes
if(drawAxes) pangolin::glDrawAxis(0.5f);

// Draw Point for measurement
glBegin(GL_POINTS);
glPointSize(5.0);
glColor3f(1.0f,0.0f,0.0f);
glVertex3f(0.0f, 0.0f, 0.0f);
glEnd();

glBegin(GL_LINES);
// Draw Camera for visualization
glVertex3f(0,0,0);
glVertex3f(sz*(0-cx)/fx,sz*(0-cy)/fy,sz);
glVertex3f(0,0,0);
glVertex3f(sz*(0-cx)/fx,sz*(height-1-cy)/fy,sz);
glVertex3f(0,0,0);
glVertex3f(sz*(width-1-cx)/fx,sz*(height-1-cy)/fy,sz);
glVertex3f(0,0,0);
glVertex3f(sz*(width-1-cx)/fx,sz*(0-cy)/fy,sz);

glVertex3f(sz*(width-1-cx)/fx,sz*(0-cy)/fy,sz);
glVertex3f(sz*(width-1-cx)/fx,sz*(height-1-cy)/fy,sz);

glVertex3f(sz*(width-1-cx)/fx,sz*(height-1-cy)/fy,sz);
glVertex3f(sz*(0-cx)/fx,sz*(height-1-cy)/fy,sz);

glVertex3f(sz*(0-cx)/fx,sz*(height-1-cy)/fy,sz);
glVertex3f(sz*(0-cx)/fx,sz*(0-cy)/fy,sz);

glVertex3f(sz*(0-cx)/fx,sz*(0-cy)/fy,sz);
glVertex3f(sz*(width-1-cx)/fx,sz*(0-cy)/fy,sz);

glEnd();




glPopMatrix();

}

glPopMatrix();
/*END ADAM: Draw Prediction (Kalman filter) */


}
Expand Down
5 changes: 3 additions & 2 deletions 03_Application/dso/src/IOWrapper/Pangolin/KeyFrameDisplay.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,13 +81,14 @@ class KeyFrameDisplay
bool refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity);

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

int id;
bool active;
SE3 camToWorld;
SE3 camToWorld_predicted; // ADAM: Changed to display kalman Filter
SE3 camToWorld_predicted; // ADAM: Added to display kalman Filter
bool hasPrediction; // ADAM: Added to display kalman Filter

inline bool operator < (const KeyFrameDisplay& other) const
{
Expand Down
35 changes: 27 additions & 8 deletions 03_Application/dso/src/IOWrapper/Pangolin/PangolinDSOViewer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,6 +132,8 @@ void PangolinDSOViewer::run()
pangolin::Var<bool> settings_showFullTrajectory("ui.FullTrajectory",false,true);
pangolin::Var<bool> settings_showActiveConstraints("ui.ActiveConst",true,true);
pangolin::Var<bool> settings_showAllConstraints("ui.AllConst",false,true);
pangolin::Var<bool> settings_showWorldGrid("ui.showWorldGrid",true,true);
pangolin::Var<bool> settings_showAxes("ui.showCoordinateSystems",true,true);


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

//ADAM: Changed (commented back in):
//pangolin::glDrawColouredCube();
glLineWidth(lineWidth*2.0f);
pangolin::glDrawAxis(1.0f);
float lineWidth = 1.0f;
glLineWidth(lineWidth*4.0f);
pangolin::glDrawAxis(5.0f);

//DrawGrid:
if(this->settings_showWorldGrid)
{
glLineWidth(lineWidth);
int gridSize = 10;
glBegin(GL_LINES);
for(int x = -gridSize; x <= gridSize; x++)
{
glVertex2f(x, -gridSize);
glVertex2f(x, gridSize);
}
for(int y = -gridSize; y <= gridSize; y++)
{
glVertex2f(-gridSize, y);
glVertex2f(gridSize, y);
}
glEnd();
}
//END ADAM

int refreshed=0;
for(KeyFrameDisplay* fh : keyframes)
{
float blue[3] = {0,0,1};
if(this->settings_showKFCameras) fh->drawCam(1,blue,0.1);
if(this->settings_showKFCameras) fh->drawCam(1,blue,0.1, this->settings_showAxes);


refreshed =+ (int)(fh->refreshPC(refreshed < 10, this->settings_scaledVarTH, this->settings_absVarTH,
this->settings_pointCloudMode, this->settings_minRelBS, this->settings_sparsity));
fh->drawPC(1);
}
if(this->settings_showCurrentCamera) currentCam->drawCam(2,0,0.2);
if(this->settings_showCurrentCamera) currentCam->drawCam(2,0,0.2, this->settings_showAxes);
drawConstraints();
lk3d.unlock();
}
Expand Down Expand Up @@ -258,6 +279,8 @@ void PangolinDSOViewer::run()
this->settings_showKFCameras = settings_showKFCameras.Get();
this->settings_showTrajectory = settings_showTrajectory.Get();
this->settings_showFullTrajectory = settings_showFullTrajectory.Get();
this->settings_showWorldGrid = settings_showWorldGrid.Get();
this->settings_showAxes = settings_showAxes.Get();

setting_render_display3D = settings_show3D.Get();
setting_render_displayDepth = settings_showLiveDepth.Get();
Expand Down Expand Up @@ -422,10 +445,6 @@ void PangolinDSOViewer::drawConstraints()
}






void PangolinDSOViewer::publishGraph(const std::map<uint64_t,Eigen::Vector2i> &connectivity)
{
if(!setting_render_display3D) return;
Expand Down
3 changes: 3 additions & 0 deletions 03_Application/dso/src/IOWrapper/Pangolin/PangolinDSOViewer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,9 @@ class PangolinDSOViewer : public Output3DWrapper
bool settings_showFullTrajectory;
bool settings_showActiveConstraints;
bool settings_showAllConstraints;
/* ADAM: UI controls to show worldGrid and Coordinate Systems */
bool settings_showWorldGrid;
bool settings_showAxes;

float settings_scaledVarTH;
float settings_absVarTH;
Expand Down

0 comments on commit 566b38c

Please sign in to comment.