810 lines
19 KiB
Text
810 lines
19 KiB
Text
|
/*
|
||
|
|
* debugview implementation
|
||
|
|
*
|
||
|
|
* Copyright (C) Li Wei, Li Ming
|
||
|
|
*
|
||
|
|
* Released under the GPL version 3.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @file
|
||
|
|
* @brief Main programm for dynamic Velodyne SLAM
|
||
|
|
*
|
||
|
|
* @author Li Wei, Wuhan University, China
|
||
|
|
* @author Li Ming, Wuhan University, China
|
||
|
|
*/
|
||
|
|
|
||
|
|
#ifdef _MSC_VER
|
||
|
|
#ifdef OPENMP
|
||
|
|
#define _OPENMP
|
||
|
|
#endif
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#ifdef _MSC_VER
|
||
|
|
#include <windows.h>
|
||
|
|
#else
|
||
|
|
#include <dlfcn.h>
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#include <fstream>
|
||
|
|
using std::ifstream;
|
||
|
|
using std::ofstream;
|
||
|
|
|
||
|
|
#include <iostream>
|
||
|
|
using std::cout;
|
||
|
|
using std::cerr;
|
||
|
|
using std::endl;
|
||
|
|
|
||
|
|
#include <sstream>
|
||
|
|
using std::stringstream;
|
||
|
|
|
||
|
|
#include "slam6d/scan.h"
|
||
|
|
#include "slam6d/Boctree.h"
|
||
|
|
#include "veloslam/veloscan.h"
|
||
|
|
#include "veloslam/trackermanager.h"
|
||
|
|
#include "veloslam/debugview.h"
|
||
|
|
#include "veloslam/velodefs.h"
|
||
|
|
|
||
|
|
#include <boost/thread.hpp>
|
||
|
|
#include <boost/thread/mutex.hpp>
|
||
|
|
#include <boost/thread/condition.hpp>
|
||
|
|
|
||
|
|
using namespace boost;
|
||
|
|
using boost::thread;
|
||
|
|
boost::mutex draw_mutex;
|
||
|
|
|
||
|
|
extern TrackerManager trackMgr;
|
||
|
|
extern VeloScan* g_pfirstScan;
|
||
|
|
|
||
|
|
#ifdef _OPENMP
|
||
|
|
#include <omp.h>
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#ifdef _MSC_VER
|
||
|
|
#define strcasecmp _stricmp
|
||
|
|
#define strncasecmp _strnicmp
|
||
|
|
#else
|
||
|
|
#include <strings.h>
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#include <cstring>
|
||
|
|
using std::flush;
|
||
|
|
|
||
|
|
#include <GL/gl.h> /* OpenGL header file */
|
||
|
|
#include <GL/glu.h> /* OpenGL utilities header file */
|
||
|
|
|
||
|
|
#ifdef _MSC_VER
|
||
|
|
#include <GL/glut.h>
|
||
|
|
#else
|
||
|
|
#include <GL/freeglut.h>
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#include "veloslam/color_util.h"
|
||
|
|
|
||
|
|
bool DebugDrawFinished =false;
|
||
|
|
bool ICPFinished =false;
|
||
|
|
bool save_animation=false;
|
||
|
|
int anim_frame_rate;
|
||
|
|
extern int scanCount;
|
||
|
|
GLenum buffermode_debugView = GL_BACK;
|
||
|
|
|
||
|
|
boost::mutex keymutex;
|
||
|
|
boost::condition keycond;
|
||
|
|
GLenum doubleBuffer;
|
||
|
|
|
||
|
|
int g_frame=0;
|
||
|
|
float rotX = 0.0, rotY = 0.0;
|
||
|
|
|
||
|
|
int x_lbefore,y_lbefore;
|
||
|
|
int x_rbefore,y_rbefore;
|
||
|
|
int z_before1,z_before2;
|
||
|
|
|
||
|
|
bool buttonSaveLeft, buttonSaveMiddle , buttonSaveRight;
|
||
|
|
float x_move,y_move,z_move;
|
||
|
|
float x_move_save,y_move_save, z_move_save;
|
||
|
|
float x_rotate,y_rotate,z_rotate;
|
||
|
|
float x_rotate_save,y_rotate_save,z_rotate_save;
|
||
|
|
float m_zoom;
|
||
|
|
|
||
|
|
float m_aspect;
|
||
|
|
|
||
|
|
// gluLookAt(0, 0, 80, 0, 0, 0, 0, 1, 0);
|
||
|
|
float m_eyex, m_eyey, m_eyez;
|
||
|
|
float m_centerx, m_centery, m_centerz;
|
||
|
|
float m_upx, m_upy, m_upz;
|
||
|
|
|
||
|
|
bool g_pause=true;
|
||
|
|
|
||
|
|
void DrawPointsRGB(Point p, float r, float g, float b)
|
||
|
|
{
|
||
|
|
GLdouble dVect1[3];
|
||
|
|
glColor3d(r, g, b);
|
||
|
|
|
||
|
|
dVect1[0]=p.x;
|
||
|
|
dVect1[1]=p.y;
|
||
|
|
dVect1[2]=p.z;
|
||
|
|
|
||
|
|
glVertex3dv(dVect1);
|
||
|
|
}
|
||
|
|
|
||
|
|
void DrawText(float x, float y, float z, char * outputstring)
|
||
|
|
{
|
||
|
|
#ifdef _MSC_VER
|
||
|
|
glPushMatrix();
|
||
|
|
glColor3f(1.0f,1.0f,1.0f);
|
||
|
|
wglUseFontBitmaps(wglGetCurrentDC(), 0, 255, 100);
|
||
|
|
|
||
|
|
glListBase(100);
|
||
|
|
glRasterPos3f(x, y, z);
|
||
|
|
glCallLists( strlen(outputstring), GL_UNSIGNED_BYTE, outputstring);
|
||
|
|
|
||
|
|
glPopMatrix();
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
void DrawTextRGB(float x, float y, float z, float r, float g, float b, char * outputstring)
|
||
|
|
{
|
||
|
|
#ifdef _MSC_VER
|
||
|
|
glPushMatrix();
|
||
|
|
glColor3f(r,g,b);
|
||
|
|
wglUseFontBitmaps(wglGetCurrentDC(), 0, 255, 100);
|
||
|
|
|
||
|
|
glListBase(100);
|
||
|
|
glRasterPos3f(x, y, z);
|
||
|
|
glCallLists( strlen(outputstring), GL_UNSIGNED_BYTE, outputstring);
|
||
|
|
|
||
|
|
glPopMatrix();
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
void DrawTextRGB(Point P, float r, float g, float b, char * outputstring)
|
||
|
|
{
|
||
|
|
#ifdef _MSC_VER
|
||
|
|
glPushMatrix();
|
||
|
|
glColor3f(r,g,b);
|
||
|
|
wglUseFontBitmaps(wglGetCurrentDC(), 0, 255, 100);
|
||
|
|
|
||
|
|
glListBase(100);
|
||
|
|
glRasterPos3f(P.x, P.y, P.z);
|
||
|
|
glCallLists( strlen(outputstring), GL_UNSIGNED_BYTE, outputstring);
|
||
|
|
|
||
|
|
glPopMatrix();
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
void Draw_Line_GL_RGB(float x1, float y1, float z1,
|
||
|
|
float x2, float y2, float z2,
|
||
|
|
float r,float g,float b,float width)
|
||
|
|
{
|
||
|
|
GLdouble dVect1[3];
|
||
|
|
GLdouble dVect2[3];
|
||
|
|
|
||
|
|
glLineWidth(width);
|
||
|
|
glBegin(GL_LINES);
|
||
|
|
glColor3d(r, g, b);
|
||
|
|
|
||
|
|
////////////////////////
|
||
|
|
dVect1[0]=x1;
|
||
|
|
dVect1[1]=y1;
|
||
|
|
dVect1[2]=z1;
|
||
|
|
glVertex3dv(dVect1);
|
||
|
|
|
||
|
|
dVect2[0]=x2;
|
||
|
|
dVect2[1]=y2;
|
||
|
|
dVect2[2]=z2;
|
||
|
|
glVertex3dv(dVect2);
|
||
|
|
|
||
|
|
glEnd();
|
||
|
|
}
|
||
|
|
|
||
|
|
void Draw_Line_GL_RGB(
|
||
|
|
Point P1, Point P2,
|
||
|
|
int width,
|
||
|
|
float r,float g,float b,
|
||
|
|
bool arrow)
|
||
|
|
{
|
||
|
|
GLdouble dVect1[3];
|
||
|
|
GLdouble dVect2[3];
|
||
|
|
|
||
|
|
glLineWidth(width);
|
||
|
|
glBegin(GL_LINES);
|
||
|
|
glColor3d(r, g, b);
|
||
|
|
|
||
|
|
dVect1[0]=P1.x;
|
||
|
|
dVect1[1]=P1.y;
|
||
|
|
dVect1[2]=P1.z;
|
||
|
|
glVertex3dv(dVect1);
|
||
|
|
|
||
|
|
dVect2[0]=P2.x;
|
||
|
|
dVect2[1]=P2.y;
|
||
|
|
dVect2[2]=P2.z;
|
||
|
|
glVertex3dv(dVect2);
|
||
|
|
|
||
|
|
glEnd();
|
||
|
|
|
||
|
|
if(arrow)
|
||
|
|
{
|
||
|
|
glPushMatrix();
|
||
|
|
glTranslatef(1.0, 0.0f, 0.0f);
|
||
|
|
glRotatef(90.0f,0.0f,1.0f,0.0f);
|
||
|
|
glutWireCone(0.027,0.09,10,10);
|
||
|
|
glPopMatrix();
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void Draw_Line_GL(float x1, float y1, float z1,float x2, float y2, float z2)
|
||
|
|
{
|
||
|
|
Draw_Line_GL_RGB(x1,y1,z1,
|
||
|
|
x2,y2,z2,
|
||
|
|
0,1.0,0);
|
||
|
|
}
|
||
|
|
|
||
|
|
void Draw_Cube_GL_RGB(float min_x, float min_y, float min_z,
|
||
|
|
float max_x, float max_y, float max_z,
|
||
|
|
float r,float g,float b)
|
||
|
|
{
|
||
|
|
Draw_Line_GL_RGB(min_x,min_y,max_z,max_x,min_y,max_z,r,g,b);
|
||
|
|
Draw_Line_GL_RGB(min_x,max_y,max_z,max_x,max_y,max_z,r,g,b);
|
||
|
|
Draw_Line_GL_RGB(min_x,max_y,min_z,max_x,max_y,min_z,r,g,b);
|
||
|
|
Draw_Line_GL_RGB(min_x,min_y,min_z,max_x,min_y,min_z,r,g,b);
|
||
|
|
|
||
|
|
Draw_Line_GL_RGB(min_x,min_y,max_z,min_x,max_y,max_z,r,g,b);
|
||
|
|
Draw_Line_GL_RGB(max_x,min_y,max_z,max_x,max_y,max_z,r,g,b);
|
||
|
|
Draw_Line_GL_RGB(min_x,min_y,min_z,min_x,max_y,min_z,r,g,b);
|
||
|
|
Draw_Line_GL_RGB(max_x,min_y,min_z,max_x,max_y,min_z,r,g,b);
|
||
|
|
|
||
|
|
Draw_Line_GL_RGB(min_x,min_y,min_z,min_x,min_y,max_z,r,g,b);
|
||
|
|
Draw_Line_GL_RGB(max_x,min_y,min_z,max_x,min_y,max_z,r,g,b);
|
||
|
|
Draw_Line_GL_RGB(max_x,max_y,min_z,max_x,max_y,max_z,r,g,b);
|
||
|
|
Draw_Line_GL_RGB(min_x,max_y,min_z,min_x,max_y,max_z,r,g,b);
|
||
|
|
}
|
||
|
|
|
||
|
|
void Draw_Inclined_Cube_GL_RGB(double rectangleVexPos[4][2],double min_z,double max_z,
|
||
|
|
float r,float g,float b,float width)
|
||
|
|
{
|
||
|
|
Draw_Line_GL_RGB(rectangleVexPos[0][0],rectangleVexPos[0][1],min_z,rectangleVexPos[1][0],rectangleVexPos[1][1],min_z,r,g,b,width);
|
||
|
|
Draw_Line_GL_RGB(rectangleVexPos[1][0],rectangleVexPos[1][1],min_z,rectangleVexPos[2][0],rectangleVexPos[2][1],min_z,r,g,b,width);
|
||
|
|
Draw_Line_GL_RGB(rectangleVexPos[2][0],rectangleVexPos[2][1],min_z,rectangleVexPos[3][0],rectangleVexPos[3][1],min_z,r,g,b,width);
|
||
|
|
Draw_Line_GL_RGB(rectangleVexPos[3][0],rectangleVexPos[3][1],min_z,rectangleVexPos[0][0],rectangleVexPos[0][1],min_z,r,g,b,width);
|
||
|
|
|
||
|
|
Draw_Line_GL_RGB(rectangleVexPos[0][0],rectangleVexPos[0][1],max_z,rectangleVexPos[1][0],rectangleVexPos[1][1],max_z,r,g,b,width);
|
||
|
|
Draw_Line_GL_RGB(rectangleVexPos[1][0],rectangleVexPos[1][1],max_z,rectangleVexPos[2][0],rectangleVexPos[2][1],max_z,r,g,b,width);
|
||
|
|
Draw_Line_GL_RGB(rectangleVexPos[2][0],rectangleVexPos[2][1],max_z,rectangleVexPos[3][0],rectangleVexPos[3][1],max_z,r,g,b,width);
|
||
|
|
Draw_Line_GL_RGB(rectangleVexPos[3][0],rectangleVexPos[3][1],max_z,rectangleVexPos[0][0],rectangleVexPos[0][1],max_z,r,g,b,width);
|
||
|
|
|
||
|
|
Draw_Line_GL_RGB(rectangleVexPos[0][0],rectangleVexPos[0][1],min_z,rectangleVexPos[0][0],rectangleVexPos[0][1],max_z,r,g,b,width);
|
||
|
|
Draw_Line_GL_RGB(rectangleVexPos[1][0],rectangleVexPos[1][1],min_z,rectangleVexPos[1][0],rectangleVexPos[1][1],max_z,r,g,b,width);
|
||
|
|
Draw_Line_GL_RGB(rectangleVexPos[2][0],rectangleVexPos[2][1],min_z,rectangleVexPos[2][0],rectangleVexPos[2][1],max_z,r,g,b,width);
|
||
|
|
Draw_Line_GL_RGB(rectangleVexPos[3][0],rectangleVexPos[3][1],min_z,rectangleVexPos[3][0],rectangleVexPos[3][1],max_z,r,g,b,width);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void Draw_Cube_GL_RGB(clusterFeature&f, float r,float g,float b)
|
||
|
|
{
|
||
|
|
Draw_Cube_GL_RGB(f.min_x,
|
||
|
|
f.min_y,
|
||
|
|
f.min_z,
|
||
|
|
f.max_x,
|
||
|
|
f.max_y,
|
||
|
|
f.max_z,r,g,b);
|
||
|
|
}
|
||
|
|
|
||
|
|
void DrawPoint(Point p, int size , float r, float g, float b)
|
||
|
|
{
|
||
|
|
GLdouble dVect1[3];
|
||
|
|
glPointSize(size);
|
||
|
|
glBegin(GL_POINTS);
|
||
|
|
glColor3d(r, g, b);
|
||
|
|
dVect1[0]=p.x;
|
||
|
|
dVect1[1]=p.y;
|
||
|
|
dVect1[2]=p.z;
|
||
|
|
glVertex3dv(dVect1);
|
||
|
|
glEnd();
|
||
|
|
}
|
||
|
|
|
||
|
|
void DrawPoint(Point p, int size , float r, float g, float b, double deltaMat[16])
|
||
|
|
{
|
||
|
|
GLdouble dVect1[3];
|
||
|
|
glPointSize(size);
|
||
|
|
glBegin(GL_POINTS);
|
||
|
|
|
||
|
|
p.transform(deltaMat);
|
||
|
|
glColor3d(r, g, b);
|
||
|
|
dVect1[0]=p.x;
|
||
|
|
dVect1[1]=p.y;
|
||
|
|
dVect1[2]=p.z;
|
||
|
|
glVertex3dv(dVect1);
|
||
|
|
glEnd();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// change the mat for the firstScan coodration.
|
||
|
|
void GetCurrecntdelteMat(Scan& CurrentScan , double *deltaMat)
|
||
|
|
{
|
||
|
|
//Point p, q;
|
||
|
|
double tempMat[16];
|
||
|
|
double temp[16]=
|
||
|
|
{ 1, 0, 0, 0,
|
||
|
|
0, 1, 0, 0,
|
||
|
|
0, 0, 1, 0,
|
||
|
|
0, 0, 0, 1} ;
|
||
|
|
|
||
|
|
M4inv(temp, tempMat);
|
||
|
|
MMult(CurrentScan.get_transMat(), tempMat, deltaMat);
|
||
|
|
}
|
||
|
|
|
||
|
|
int DrawAll_ScanPoints_Number(vector <Scan *> allScans, int psize, float r, float g, float b, int n)
|
||
|
|
{
|
||
|
|
int i,j,k,colorIdx;
|
||
|
|
Point p;
|
||
|
|
double deltaMat[16];
|
||
|
|
|
||
|
|
for(int i =0; i <n ;i ++)
|
||
|
|
{
|
||
|
|
Scan *CurrentScan = allScans[i];
|
||
|
|
|
||
|
|
DataXYZ xyz(CurrentScan->get("xyz"));
|
||
|
|
DataType Pt(CurrentScan->get("type"));
|
||
|
|
|
||
|
|
GetCurrecntdelteMat(*CurrentScan , deltaMat);
|
||
|
|
int size =xyz.size();
|
||
|
|
|
||
|
|
for(j=0; j <size; j++)
|
||
|
|
{
|
||
|
|
// need more complex classification
|
||
|
|
p.x = xyz[j][0];
|
||
|
|
p.y = xyz[j][1];
|
||
|
|
p.z = xyz[j][2];
|
||
|
|
p.type = Pt[j];
|
||
|
|
|
||
|
|
p.transform(deltaMat);
|
||
|
|
|
||
|
|
if (p.type & POINT_TYPE_GROUND) //ground
|
||
|
|
DrawPoint(p, psize , 0.8, 0.8, 0.8, deltaMat);
|
||
|
|
if (p.type & POINT_TYPE_STATIC_OBJECT) //static
|
||
|
|
DrawPoint(p, psize , 0.0, 0.0, 0.8, deltaMat);
|
||
|
|
if (p.type & POINT_TYPE_MOVING_OBJECT) //moving
|
||
|
|
DrawPoint(p, psize , 0.0, 0.8, 0.0, deltaMat);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void glDumpWindowPPM_debugView(const char *filename, GLenum mode)
|
||
|
|
{
|
||
|
|
int win_height, win_width;
|
||
|
|
int i,j,k,l; // Counter variables
|
||
|
|
GLubyte *buffer; // The GL Frame Buffer
|
||
|
|
unsigned char *ibuffer; // The PPM Output Buffer
|
||
|
|
ofstream fp; // The PPM File
|
||
|
|
|
||
|
|
GLint viewport[4];
|
||
|
|
glGetIntegerv(GL_VIEWPORT, viewport);
|
||
|
|
win_height = viewport[3];
|
||
|
|
win_width = viewport[2];
|
||
|
|
|
||
|
|
// Allocate memory for the the frame buffer and output buffer
|
||
|
|
buffer = new GLubyte[win_width * win_height * RGBA];
|
||
|
|
ibuffer = new unsigned char[win_width * win_height * RGB];
|
||
|
|
|
||
|
|
// Read window contents from GL frame buffer with glReadPixels
|
||
|
|
glFinish();
|
||
|
|
glReadBuffer(buffermode_debugView);
|
||
|
|
glReadPixels(0, 0, win_width, win_height,
|
||
|
|
GL_RGBA, GL_UNSIGNED_BYTE, buffer);
|
||
|
|
|
||
|
|
// glReadPixels(0, 0, win_width, win_height,
|
||
|
|
// GL_RGB, GL_UNSIGNED_BYTE, buffer);
|
||
|
|
|
||
|
|
// Open the output file
|
||
|
|
fp.open(filename, ios::out);
|
||
|
|
|
||
|
|
// Write a proper P6 PPM header
|
||
|
|
fp << "P6" << endl << "# CREATOR: 3D_Viewer by Andreas Nuechter, University of Osnabrueck"
|
||
|
|
<< endl << win_width << " " << win_height << " " << UCHAR_MAX << endl;
|
||
|
|
|
||
|
|
// Loop through the frame buffer data, writing to the PPM file. Be careful
|
||
|
|
// to account for the frame buffer having 4 bytes per pixel while the
|
||
|
|
// output file has 3 bytes per pixel
|
||
|
|
l = 0;
|
||
|
|
for (i = 0; i < win_height; i++)
|
||
|
|
{ // For each row
|
||
|
|
for (j = 0; j < win_width; j++)
|
||
|
|
{ // For each column
|
||
|
|
// for (k = 0; k < RGB; k++)
|
||
|
|
{ // For each RGB component
|
||
|
|
//cout << (RGBA*((win_height-1-i)*win_width+j)+k) << endl;
|
||
|
|
ibuffer[l++] = (unsigned char) *(buffer + (RGBA*((win_height-1-i)*win_width+j)+1));
|
||
|
|
ibuffer[l++] = (unsigned char) *(buffer + (RGBA*((win_height-1-i)*win_width+j)+2));
|
||
|
|
ibuffer[l++] = (unsigned char) *(buffer + (RGBA*((win_height-1-i)*win_width+j)+0));
|
||
|
|
} // end RGB
|
||
|
|
} // end column
|
||
|
|
} // end row
|
||
|
|
|
||
|
|
// to make a video do:
|
||
|
|
// for f in *ppm ; do convert -quality 100 $f `basename $f ppm`jpg; done
|
||
|
|
// mencoder "mf://*.jpg" -mf fps=10 -o test.avi -ovc lavc -lavcopts vcodec=msmpeg4v2:vbitrate=800
|
||
|
|
// Write output buffer to the file */
|
||
|
|
fp.write((const char*)ibuffer, sizeof(unsigned char) * (RGB * win_width * win_height));
|
||
|
|
fp.close();
|
||
|
|
fp.clear();
|
||
|
|
delete [] buffer;
|
||
|
|
delete [] ibuffer;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
static void Reshape(int w, int h)
|
||
|
|
{
|
||
|
|
glViewport(0, 0, (GLint)w, (GLint)h);
|
||
|
|
|
||
|
|
m_aspect = (GLfloat) w / (GLfloat) h;
|
||
|
|
|
||
|
|
glMatrixMode(GL_PROJECTION);
|
||
|
|
glLoadIdentity();
|
||
|
|
|
||
|
|
gluPerspective(45.0f,
|
||
|
|
m_aspect,
|
||
|
|
0.0f,
|
||
|
|
4000.0f);
|
||
|
|
|
||
|
|
glMatrixMode(GL_MODELVIEW);
|
||
|
|
glLoadIdentity();
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
#define KEY_ESC 27
|
||
|
|
static void Key(unsigned char key, int x, int y)
|
||
|
|
{
|
||
|
|
switch (key)
|
||
|
|
{
|
||
|
|
case KEY_ESC:
|
||
|
|
exit(0);
|
||
|
|
break;
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void MouseMove(int x, int y)
|
||
|
|
{
|
||
|
|
int mod = glutGetModifiers();
|
||
|
|
switch(mod)
|
||
|
|
{
|
||
|
|
|
||
|
|
case GLUT_ACTIVE_CTRL :
|
||
|
|
x_rotate += (y - z_move_save)/100;
|
||
|
|
if (x_rotate > 360)
|
||
|
|
x_rotate=x_rotate - 360;
|
||
|
|
if (x_rotate < -360)
|
||
|
|
x_rotate=x_rotate + 360;
|
||
|
|
cout << "Ctrl Held" << endl;
|
||
|
|
return;
|
||
|
|
|
||
|
|
case GLUT_ACTIVE_SHIFT :
|
||
|
|
y_rotate += (y - z_move_save)/100;
|
||
|
|
if (y_rotate > 360)
|
||
|
|
y_rotate=y_rotate - 360;
|
||
|
|
if (y_rotate < -360)
|
||
|
|
y_rotate=y_rotate + 360;
|
||
|
|
|
||
|
|
cout << "Shift Held" << endl;
|
||
|
|
return;
|
||
|
|
|
||
|
|
case GLUT_ACTIVE_ALT :
|
||
|
|
float temp = (x - x_move_save)/100;
|
||
|
|
z_rotate += atanf(temp);
|
||
|
|
|
||
|
|
cout << "Alt Held" << endl;
|
||
|
|
return;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
if(buttonSaveLeft)
|
||
|
|
{
|
||
|
|
x_move += (x - x_move_save)/100;
|
||
|
|
z_move += (y - z_move_save)/100;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(buttonSaveMiddle)
|
||
|
|
{
|
||
|
|
float multiplay = (y - z_move_save)/1000;
|
||
|
|
m_zoom =m_zoom*(1+multiplay);
|
||
|
|
}
|
||
|
|
|
||
|
|
if(buttonSaveRight)
|
||
|
|
{
|
||
|
|
float multiplay = (y - z_move_save)/1000;
|
||
|
|
m_zoom =m_zoom*(1+multiplay);
|
||
|
|
}
|
||
|
|
|
||
|
|
cout << "mouse move " << buttonSaveLeft << buttonSaveMiddle <<buttonSaveRight << " "
|
||
|
|
<< x << " "<< y <<endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
static void PassiveMouseMove(int x, int y)
|
||
|
|
{
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
static void MouseRotate(int x, int y, int z)
|
||
|
|
{
|
||
|
|
cout << "mouse Rotate " << x << " "<< y << " "<< z <<endl;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
static void MouseKey(int button, int state, int x, int y)
|
||
|
|
{
|
||
|
|
x_move_save=x;
|
||
|
|
y_move_save;
|
||
|
|
z_move_save=y;
|
||
|
|
|
||
|
|
switch (button)
|
||
|
|
{
|
||
|
|
case GLUT_LEFT_BUTTON:
|
||
|
|
if(state == GLUT_DOWN)
|
||
|
|
buttonSaveLeft=true;
|
||
|
|
else
|
||
|
|
buttonSaveLeft=false;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_MIDDLE_BUTTON:
|
||
|
|
if(state == GLUT_DOWN)
|
||
|
|
buttonSaveMiddle=true;
|
||
|
|
else
|
||
|
|
buttonSaveMiddle=false;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_RIGHT_BUTTON:
|
||
|
|
if(state == GLUT_DOWN)
|
||
|
|
buttonSaveRight=true;
|
||
|
|
else
|
||
|
|
buttonSaveRight=false;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void SpecialKey(int key, int x, int y)
|
||
|
|
{
|
||
|
|
switch (key)
|
||
|
|
{
|
||
|
|
case GLUT_KEY_UP:
|
||
|
|
z_move --;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_DOWN:
|
||
|
|
z_move ++;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_LEFT:
|
||
|
|
x_move --;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_RIGHT:
|
||
|
|
x_move ++;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_PAGE_UP:
|
||
|
|
m_zoom= 1.1*m_zoom;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_PAGE_DOWN :
|
||
|
|
m_zoom =m_zoom/1.1;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_HOME:
|
||
|
|
m_zoom=1.5*m_zoom;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_END:
|
||
|
|
m_zoom=m_zoom/1.5;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_F1:
|
||
|
|
x_rotate += 3;
|
||
|
|
if (x_rotate > 360)
|
||
|
|
x_rotate=x_rotate - 360;
|
||
|
|
if (x_rotate < -360)
|
||
|
|
x_rotate=x_rotate + 360;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_F2:
|
||
|
|
x_rotate += -3;
|
||
|
|
if (x_rotate > 360)
|
||
|
|
x_rotate=x_rotate - 360;
|
||
|
|
if (x_rotate < -360)
|
||
|
|
x_rotate=x_rotate + 360;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_F3:
|
||
|
|
y_rotate += 3;
|
||
|
|
if (y_rotate > 360)
|
||
|
|
y_rotate=y_rotate - 360;
|
||
|
|
if (y_rotate < -360)
|
||
|
|
y_rotate=y_rotate + 360;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_F4 :
|
||
|
|
y_rotate += -3;
|
||
|
|
if (y_rotate > 360)
|
||
|
|
y_rotate=y_rotate - 360;
|
||
|
|
if (y_rotate < -360)
|
||
|
|
y_rotate=y_rotate + 360;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_F5:
|
||
|
|
z_rotate += atanf(3);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_F6:
|
||
|
|
z_rotate += atanf(-3);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_F7:
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_F8:
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_F11:
|
||
|
|
printf("paseu\n");
|
||
|
|
g_pause != g_pause;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GLUT_KEY_F12:
|
||
|
|
printf("next\n");
|
||
|
|
keycond.notify_one();
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
glutPostRedisplay();
|
||
|
|
}
|
||
|
|
|
||
|
|
static void Draw(void)
|
||
|
|
{
|
||
|
|
DebugDrawFinished =false;
|
||
|
|
glClearColor(1.0, 1.0, 1.0, 1.0);
|
||
|
|
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
|
||
|
|
glClearColor(1.0, 1.0, 1.0, 1.0);
|
||
|
|
glLoadIdentity();
|
||
|
|
|
||
|
|
gluLookAt(m_eyex, m_eyey, m_eyez,
|
||
|
|
m_centerx, m_centery, m_centerz,
|
||
|
|
m_upx, m_upy, m_upz);
|
||
|
|
|
||
|
|
glRotatef(x_rotate,1,0,0);
|
||
|
|
glRotatef(y_rotate,0,1,0);
|
||
|
|
glRotatef(z_rotate,0,0,1);
|
||
|
|
|
||
|
|
glTranslatef(x_move,y_move,z_move);
|
||
|
|
glScalef(m_zoom, m_zoom, m_zoom);
|
||
|
|
|
||
|
|
int scansize = Scan::allScans.size();
|
||
|
|
Point p1, p2, p3;
|
||
|
|
|
||
|
|
if (scansize <5)
|
||
|
|
{
|
||
|
|
DebugDrawFinished =true;
|
||
|
|
return ;
|
||
|
|
}
|
||
|
|
|
||
|
|
Scan *CurrentScan = Scan::allScans[3];
|
||
|
|
if (CurrentScan == NULL)
|
||
|
|
{
|
||
|
|
DebugDrawFinished =true;
|
||
|
|
return ;
|
||
|
|
}
|
||
|
|
|
||
|
|
double deltaMat[16];
|
||
|
|
double deltaMatNext[16];
|
||
|
|
GetCurrecntdelteMat(*CurrentScan , deltaMat);
|
||
|
|
|
||
|
|
double PosTheta[3];
|
||
|
|
double Pos[3];
|
||
|
|
Matrix4ToEuler(deltaMat, PosTheta, Pos);
|
||
|
|
|
||
|
|
cout << " pose of current "
|
||
|
|
<< " " << PosTheta [0]
|
||
|
|
<< " " << PosTheta [1]
|
||
|
|
<< " " << PosTheta [2] <<endl;
|
||
|
|
|
||
|
|
// you need to find the direction of ego-vehicle.
|
||
|
|
// p1.x = -4000 * cos ( PosTheta [1]) ; p1.y= 2000; p1.z= 4000 * sin ( PosTheta [1]);
|
||
|
|
p1.x = 0 ; p1.y= 15000; p1.z= 0;
|
||
|
|
p2.x = 0; p2.y= 0; p2.z= 0;
|
||
|
|
p3.x = 0; p3.y=200; p3.z= 0;
|
||
|
|
|
||
|
|
p1.transform(deltaMat);
|
||
|
|
p2.transform(deltaMat);
|
||
|
|
p3.transform(deltaMat);
|
||
|
|
|
||
|
|
gluLookAt(p1.x, p1.y, p1.z,
|
||
|
|
p2.x, p2.y, p2.z,
|
||
|
|
p3.x, p3.y, p3.z);
|
||
|
|
|
||
|
|
DrawAll_ScanPoints_Number(Scan::allScans, 1, 0.8, 0.8, 0.8, scansize-1);
|
||
|
|
trackMgr.DrawTrackersMovtion_Long_Number_All(Scan::allScans, scansize-1);
|
||
|
|
trackMgr.DrawEgoTrajectory();
|
||
|
|
glFlush();
|
||
|
|
glutSwapBuffers();
|
||
|
|
|
||
|
|
if(save_animation)
|
||
|
|
{
|
||
|
|
string scandirectory =".\\video\\";
|
||
|
|
string filename = scandirectory + "animframe"
|
||
|
|
+ to_string(scanCount,5) + ".ppm";
|
||
|
|
cout << "write " << filename << endl;
|
||
|
|
glDumpWindowPPM_debugView(filename.c_str(),0);
|
||
|
|
}
|
||
|
|
|
||
|
|
DebugDrawFinished =true;
|
||
|
|
}
|
||
|
|
|
||
|
|
int Show(int frameno)
|
||
|
|
{
|
||
|
|
GLenum type;
|
||
|
|
|
||
|
|
m_eyex=0, m_eyey=0, m_eyez=80;
|
||
|
|
m_centerx=0, m_centery=0, m_centerz=0;
|
||
|
|
m_upx=0, m_upy=1, m_upz=0;
|
||
|
|
|
||
|
|
buttonSaveLeft=false;
|
||
|
|
buttonSaveMiddle=false;
|
||
|
|
buttonSaveRight=false;
|
||
|
|
|
||
|
|
x_move=0.0;
|
||
|
|
y_move=0.0;
|
||
|
|
z_move=0.0;
|
||
|
|
|
||
|
|
x_rotate=90.0;
|
||
|
|
y_rotate=0.0;
|
||
|
|
z_rotate=0.0;
|
||
|
|
|
||
|
|
m_zoom=0.01;
|
||
|
|
|
||
|
|
x_lbefore=0,y_lbefore=0;
|
||
|
|
x_rbefore=0,y_rbefore=0;
|
||
|
|
z_before1=0,z_before2=0;
|
||
|
|
|
||
|
|
type = GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE;
|
||
|
|
glutInitDisplayMode(type);
|
||
|
|
|
||
|
|
glutInitWindowSize(800, 600);
|
||
|
|
glutCreateWindow("Point Cloud Viewer");
|
||
|
|
|
||
|
|
glutReshapeFunc(Reshape);
|
||
|
|
glutKeyboardFunc(Key);
|
||
|
|
glutSpecialFunc(SpecialKey);
|
||
|
|
glutMouseFunc(MouseKey);
|
||
|
|
glutMotionFunc(MouseMove);
|
||
|
|
glutPassiveMotionFunc(PassiveMouseMove);
|
||
|
|
glutSpaceballRotateFunc(MouseRotate);
|
||
|
|
|
||
|
|
g_frame = frameno;
|
||
|
|
|
||
|
|
while(ICPFinished ==false)
|
||
|
|
{
|
||
|
|
#ifdef _MSC_VER
|
||
|
|
Sleep(1);
|
||
|
|
#else
|
||
|
|
usleep(10000);
|
||
|
|
#endif
|
||
|
|
// cout << "sleep" <<endl;
|
||
|
|
}
|
||
|
|
glutDisplayFunc(Draw);
|
||
|
|
glutMainLoop();
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void ShowWraper()
|
||
|
|
{
|
||
|
|
Show(0);
|
||
|
|
}
|
||
|
|
|
||
|
|
void StartShow()
|
||
|
|
{
|
||
|
|
boost::thread t(ShowWraper);
|
||
|
|
}
|