void drawEllipseToData(ImgRGB& img, double x, double y, double var[4], uchar r,
uchar g, uchar b) {
double evec[4], eval[2];
dgeevFor(2, var, evec, eval);
// int l1 = static_cast<int> (eval[0] + 0.5);
// int l2 = static_cast<int> (eval[1] + 0.5);
double l1 = sqrt(eval[0]);
double l2 = sqrt(eval[1]);
double dx1 = l1 * evec[0];
double dy1 = l1 * evec[1];
double dx2 = l2 * evec[2];
double dy2 = l2 * evec[3];
cv::Mat cvOutImg(img.rows, img.cols, CV_8UC3, img.data);
cv::line(cvOutImg, cv::Point2f(x - dx1, y - dy1),
cv::Point2f(x + dx1, y + dy1), cv::Scalar(r, g, b), 1, CV_AA);
cv::line(cvOutImg, cv::Point2f(x - dx2, y - dy2),
cv::Point2f(x + dx2, y + dy2), cv::Scalar(r, g, b), 1, CV_AA);
//get rotation angle
double theta = atan2(evec[0], evec[1]);
cv::ellipse(cvOutImg, cv::Point2f(x, y), cv::Size(l1, l2), theta, 0, 360,
cv::Scalar(r, g, b), 1, CV_AA);
// double ivar[4];
// mat22Inv(var,ivar);
}
void drawCurMapPoint(const MapPoint* p, double range, double pointSize,
bool drawCov) {
//GLfloat LightAmbient[] = { 0.5f, 1.0f, 0.5f, 1.0f };
GLfloat LightAmbient[] = { 0.5f, 0.5f, 0.5f, 1.0f };
GLfloat LightAmbientInAct[] = { 0.5f, 0.5f, 0.5f, 1.0f };
GLfloat LightDiffuse[] = { 0.5f, 0.5f, 0.5f, 1.0f };
GLfloat LightPosition[] = { -range * 1.0f, -range * 1.0f, 0.0f, 1.0f };
glLightfv(GL_LIGHT1, GL_AMBIENT, LightAmbient);
glLightfv(GL_LIGHT1, GL_DIFFUSE, LightDiffuse);
glLightfv(GL_LIGHT1, GL_AMBIENT, LightAmbientInAct);
glLightfv(GL_POSITION, GL_DIFFUSE, LightAmbientInAct);
if( p->isFalse())
return;
glColor3f(0.2f, 0.8f, 0.2f);
//for debug
if (p->flag == FLAG_MAPPOINT_TEST4)
glColor3f(1.0f, 0.0f, 0.0f);
// glEnable(GL_LIGHTING);
// glEnable(GL_LIGHT1);
//
// glPushMatrix();
// glTranslatef(p->x, p->y, p->z);
// glutSolidSphere(0.1 * range * p->numVisCam, 12, 12);
// glPopMatrix();
//
// glDisable(GL_LIGHT1);
// glDisable(GL_LIGHTING);
if(p->isFalse())
return;
glPointSize(4.0 * pointSize);
glBegin(GL_POINTS);
glVertex3d(p->x, p->y, p->z);
glEnd();
if (drawCov && p->cov[0] > 0) {
glColor4f(0.2f, 0.8f, 0.2f, 0.2f);
// //for debug
// if (p->flag == FLAG_MAPPOINT_TEST4
// )
// glColor3f(1.0f, 0.0f, 0.0f);
double evec[9];
double eval[3];
dgeevFor(3, p->cov, evec, eval);
glPushMatrix();
glTranslatef(p->x, p->y, p->z);
double rotMat[16];
memset(rotMat, 0, sizeof(double) * 16);
memcpy(rotMat, evec, sizeof(double) * 3);
memcpy(rotMat + 4, evec + 3, sizeof(double) * 3);
memcpy(rotMat + 8, evec + 3, sizeof(double) * 3);
rotMat[15] = 1;
glMultMatrixd(rotMat);
//assert(eval[0] > 0 && eval[1] > 0 && eval[2] > 0);
glScaled(sqrt(eval[0]), sqrt(eval[1]), sqrt(eval[2]));
glutSolidSphere(1.0, 12, 12);
glPopMatrix();
}
}