void drawCamCenter(const CamPoseItem* cam, const float* color, int ptSz) {
double org[3], ox[3], oy[3], oz[3];
getCamCoords(cam, org, ox, oy, oz);
glColor3f(color[0], color[1], color[2]);
glLineWidth(ptSz);
glBegin(GL_POINTS);
glVertex3f(org[0], org[1], org[2]);
glEnd();
}
void GLScenePane::getSceneScale() {
m_center[0] = 0;
m_center[1] = 0;
m_center[2] = 0;
int num = 0;
double org[3], axisX[3], axisY[3], axisZ[3];
for (int i = 0; i < m_pSLAM->numCams; i++) {
const CamPoseItem* cam = m_pSLAM->slam[i].m_camPos.first();
while (cam) {
getCamCoords(cam, org, axisX, axisY, axisZ);
m_center[0] += org[0];
m_center[1] += org[1];
m_center[2] += org[2];
num++;
cam = cam->next;
}
}
m_center[0] /= num;
m_center[1] /= num;
m_center[2] /= num;
m_scale = 0;
for (int i = 0; i < m_pSLAM->numCams; i++) {
const CamPoseItem* cam = m_pSLAM->slam[i].m_camPos.first();
while (cam) {
getCamCoords(cam, org, axisX, axisY, axisZ);
double dx = m_center[0] - org[0];
double dy = m_center[1] - org[1];
double dz = m_center[2] - org[2];
m_scale += sqrt(dx * dx + dy * dy + dz * dz);
cam = cam->next;
}
}
m_scale /= num;
m_scale *= 0.5;
}
void drawCameraPose(const CamPoseItem* cam, double camSize, const float* color = 0) {
double p0[3], xp[3], yp[3], zp[3]; //, tmp[3];
getCamCoords(cam, p0, xp, yp, zp);
double scale = camSize;
// //draw id
// char id_str[256];
// sprintf(id_str, "%d", cam->f);
// glutPrint(cam->t[0], cam->t[1], cam->t[2], GLUT_BITMAP_9_BY_15, id_str, 1, 1, 1, 0);
//draw axes
// glBegin(GL_LINES);
// glColor3f(1, 0, 0);
// glquiver3(p0, xp, scale);
// glColor3f(0, 1, 0);
// glquiver3(p0, yp, scale);
// glColor3f(0, 0, 1);
// glquiver3(p0, zp, scale);
// glEnd();
double quad[4][3];
add(p0, zp, quad[0], 1.0, 1 * scale);
add(quad[0], xp, quad[1], 1.0, 0.8 * scale);
add(quad[1], yp, quad[0], 1.0, 0.8 * scale);
if (color) {
glColor3f(color[0], color[1], color[2]);
}
glBegin(GL_LINE_STRIP);
glpt3(quad[0]);
add(quad[0], xp, quad[1], 1.0, -1.6 * scale);
glpt3(quad[1]);
add(quad[1], yp, quad[2], 1.0, -1.6 * scale);
glpt3(quad[2]);
add(quad[2], xp, quad[3], 1.0, 1.6 * scale);
glpt3(quad[3]);
glpt3(quad[0]);
glEnd();
glBegin(GL_LINES);
glline(p0, quad[0]);
glline(p0, quad[1]);
glline(p0, quad[2]);
glline(p0, quad[3]);
glEnd();
}
void drawCamera(int IW, int IH, const double* K, const CamPoseItem* cam,
double camSize, const float* color = 0, const double maxDist = 100) {
double p0[3], xp[3], yp[3], zp[3]; //, tmp[3];
getCamCoords(cam, p0, xp, yp, zp);
double scale = camSize;
//draw id
// char id_str[256];
// sprintf(id_str, "%d", cam->f);
// glutPrint(cam->t[0], cam->t[1], cam->t[2], GLUT_BITMAP_9_BY_15, id_str, 1, 1, 1, 0);
//draw axes
glBegin(GL_LINES);
glColor3f(0, 0, 1); // x
glquiver3(p0, xp, scale);
glColor3f(0, 1, 0); // y
glquiver3(p0, yp, scale);
glColor3f(1, 0, 0); //z
glquiver3(p0, zp, scale);
//glBegin(GL_LINES);
//glColor3f(color[0],color[1],color[2]);
//glquiver3(p0, xp, scale);
//glColor3f(color[0],color[1],color[2]);
//glquiver3(p0, yp, scale);
//glColor3f(color[0],color[1],color[2]);
//glquiver3(p0, zp, scale);
glEnd();
//double quad[4][3];
//add(p0, zp, quad[0], 1.0, 1 * scale);
//add(quad[0], xp, quad[1], 1.0, 0.8 * scale);
//add(quad[1], yp, quad[0], 1.0, 0.8 * scale);
//if (color) {
// glColor3f(color[0], color[1], color[2]);
//}
//glLineWidth(2.0f);
//glBegin(GL_LINE_STRIP);
//glpt3(quad[0]);
//add(quad[0], xp, quad[1], 1.0, -1.6 * scale);
//glpt3(quad[1]);
//add(quad[1], yp, quad[2], 1.0, -1.6 * scale);
//glpt3(quad[2]);
//add(quad[2], xp, quad[3], 1.0, 1.6 * scale);
//glpt3(quad[3]);
//glpt3(quad[0]);
//glEnd();
//glBegin(GL_LINES);
//glline(p0, quad[0]);
//glline(p0, quad[1]);
//glline(p0, quad[2]);
//glline(p0, quad[3]);
//glEnd();
//draw view angle
if (maxDist > 0) {
double Cx = K[2];
double fx = K[0];
double fy = K[4];
double A1[3] = { -maxDist * Cx / fx, 0, maxDist };
double A2[3] = { (IW - Cx) * maxDist / fx, 0, maxDist };
double wA1[3];
double wA2[3];
CamCoord2WoldCoord(cam->R, cam->t, A1, wA1);
CamCoord2WoldCoord(cam->R, cam->t, A2, wA2);
//transform into world coordinates
//glColor4f(color[0], color[1], color[2], 0.2);
glColor4f(0, 0, 1, 0.2);
glBegin(GL_TRIANGLES);
glpt3(wA1);
glpt3(p0);
glpt3(wA2);
glEnd();
}
}
