void drawAllCam(const CamPoseList& camlst, double cam_size) {
CamPoseItem* cam = camlst.first();
while (cam) {
drawCameraPose(cam, cam_size, 0);
cam = cam->next;
}
}
void GLScenePane::drawCameras() {
//draw camera trajectory
for (int i = 0; i < m_pSLAM->numCams; i++) {
const CamPoseItem* cam = m_pSLAM->slam[i].m_camPos.first();
while (cam->f < m_pSLAM->m_lastFrmBundle)
cam = cam->next;
//glColor3f(CAMERA_COLORS[3 * i], CAMERA_COLORS[3 * i + 1], CAMERA_COLORS[3 * i + 2]);
glLineWidth(2.0f);
glColor3f(0.5f, 0.5f, 0.5f);
glBegin(GL_LINE_STRIP);
while (cam) {
// drawCamCenter(cam, PT_COLOR + 3 * i);
//if (cam->f >= m_pSLAM->curFrame - 200)
{
double org[3];
getCamCenter(cam, org);
glVertex3d(org[0], org[1], org[2]);
}
cam = cam->next;
}
glEnd();
glPointSize(2.0);
glBegin(GL_LINE_STRIP);
cam = m_pSLAM->slam[i].m_camPos.first();
while (cam) {
// drawCamCenter(cam, PT_COLOR + 3 * i);
//if (cam->f >= m_pSLAM->curFrame - 200)
{
double org[3];
getCamCenter(cam, org);
glVertex3d(org[0], org[1], org[2]);
}
cam = cam->next;
}
glEnd();
cam = m_pSLAM->slam[i].m_camPos.first();
glColor3f(0.5f, 0.5f, 0.5f);
glLineWidth(2.0f);
glBegin(GL_LINE_STRIP);
while (cam && cam->f <= m_pSLAM->m_lastFrmBundle) {
// drawCamCenter(cam, PT_COLOR + 3 * i);
if (cam->f >= m_pSLAM->m_firstFrmBundle) {
double org[3];
getCamCenter(cam, org);
glVertex3d(org[0], org[1], org[2]);
}
cam = cam->next;
}
glEnd();
drawCamera(m_pSLAM->slam[i].videoReader->_w,
m_pSLAM->slam[i].videoReader->_h, m_pSLAM->slam[i].K.data,
m_pSLAM->slam[i].m_camPos.current(), m_camSize,
CAMERA_COLORS + 3 * i, 100);
}
//draw key camera poses
for (int i = 0; i < m_pSLAM->numCams; i++) {
for (size_t n = 0; n < m_pSLAM->slam[i].m_selfKeyPose.size(); ++n) {
const KeyPose* keyPose = m_pSLAM->slam[i].m_selfKeyPose[n];
drawCameraPose(keyPose->cam, m_camSize, CAMERA_COLORS + 3 * i, 4);
}
const KeyPose* keyPose = m_pSLAM->slam[i].m_keyPose.first();
int n = 0;
while (keyPose) {
drawCameraPose(keyPose->cam, m_camSize * 0.5,
CAMERA_COLORS + 3 * i);
n++;
keyPose = keyPose->next;
}
}
}
