void reshape(int w, int h) {
glViewport(0,0, w,h);
matrixIdentity(Projection);
if (w > h)
matrixOrtho(Projection, -20.0, 20.0, -20.0*h/w, 20.0*w/h, -1, +1);
else
matrixOrtho(Projection, -20.0*w/h, 20.0*w/h, -20.0, 20.0, -1, +1);
matrixIdentity(ModelView);
}
// Returns a matrix to operate rotation
static C3DTMatrix matrixForRotation(const float ax, const float ay, const float az)
{
C3DTMatrix r = matrixIdentity();
float s;
float c;
if (fabs(ax) > EPSILON)
{
C3DTMatrix m = matrixIdentity();
s = sinf(ax);
c = cosf(ax);
m.flts[5] = c;
m.flts[6] = s;
m.flts[9] = -s;
m.flts[10] = c;
matrixTransform(&r, m);
}
if (fabs(ay) > EPSILON)
{
C3DTMatrix m = matrixIdentity();
s = sinf(ay);
c = cosf(ay);
m.flts[0] = c;
m.flts[2] = s;
m.flts[8] = -s;
m.flts[10] = c;
matrixTransform(&r, m);
}
if (fabs(az) > EPSILON)
{
C3DTMatrix m = matrixIdentity();
s = sinf(az);
c = cosf(az);
m.flts[0] = c;
m.flts[1] = s;
m.flts[4] = -s;
m.flts[5] = c;
matrixTransform(&r, m);
}
return r;
}
int main(int argc, char *argv[]) {
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_SINGLE | GLUT_RGBA | GLUT_DEPTH);
glutInitWindowSize(800,800);
glutInitWindowPosition(10,10);
glutCreateWindow("Super Ellipsoid");
glutDisplayFunc(display);
glutKeyboardFunc(keyboard);
glutSpecialFunc(specialKeyboard);
glutMouseFunc(mouse);
glutMotionFunc(mouseMotion);
glutIdleFunc(idle);
glutCreateMenu(displayMenu);
glutAddMenuEntry("Increase m",1);
glutAddMenuEntry("Decrease m",2);
glutAddMenuEntry("Increase n",3);
glutAddMenuEntry("Decrease n",4);
glutAddMenuEntry("Exit",5);
glutAttachMenu(GLUT_RIGHT_BUTTON);
installShaders();
setCamera();
matrixIdentity(Projection);
matrixPerspective(Projection,
40, 1.0, (GLfloat) 1, 750);
initValues();
glClearColor(0.0,0.0,0.0,1.0);
glutMainLoop();
return 0;
}
Matrix matrixTranslate(float fX, float fY, float fZ)
{
Matrix sResult = matrixIdentity();
sResult.aElem[12] = fX;
sResult.aElem[13] = fY;
sResult.aElem[14] = fZ;
return sResult;
}
void setCamera() {
sphericalToCartesian(radius, theta, phi,
&eyeX, &eyeY, &eyeZ);
eyeX += centerX; eyeY += centerY; eyeZ += centerZ;
matrixIdentity(ModelView);
matrixLookat(ModelView, eyeX, eyeY, eyeZ,
centerX, centerY, centerZ,
upX, upY, upZ);
}
// Returns a matrix to operate non-uniform scaling
static C3DTMatrix matrixForScaling(const float sx, const float sy, const float sz)
{
C3DTMatrix m = matrixIdentity();
m.flts[3] = sx;
m.flts[5] = sy;
m.flts[10] = sz;
return m;
}
// Returns a matrix to operate translations
static C3DTMatrix matrixForTranslation(const float dx, const float dy, const float dz)
{
C3DTMatrix m = matrixIdentity();
m.flts[3] = dx;
m.flts[7] = dy;
m.flts[11] = dz;
return m;
}
Matrix matrixRotateZ(int iAngle)
{
Matrix sResult = matrixIdentity();
sResult.aElem[0] = cos(DEG2RAD(iAngle));
sResult.aElem[4] = -sin(DEG2RAD(iAngle));
sResult.aElem[1] = sin(DEG2RAD(iAngle));
sResult.aElem[5] = cos(DEG2RAD(iAngle));
return sResult;
}
Matrix matrixPerspective(float fFOV, float fRatio, float fNear, float fFar)
{
Matrix sResult = matrixIdentity();
sResult.aElem[ 0] = fFOV / fRatio;
sResult.aElem[ 5] = fFOV;
sResult.aElem[10] = -(fFar + fNear) / (fFar - fNear);
sResult.aElem[14] = (-2.0f * fFar * fNear) / (fFar - fNear);
sResult.aElem[11] = -1.0f;
sResult.aElem[15] = 0.0f;
return sResult;
}
Matrix matrixOrthographic(float fLeft, float fRight, float fBottom, float fTop, float fNear, float fFar)
{
Matrix sResult = matrixIdentity();
sResult.aElem[ 0] = 2.0f / (fRight - fLeft);
sResult.aElem[12] = -(fRight + fLeft) / (fRight - fLeft);
sResult.aElem[ 5] = 2.0f / (fTop - fBottom);
sResult.aElem[13] = -(fTop + fBottom) / (fTop - fBottom);
sResult.aElem[10] = 2.0f / (fFar - fNear);
sResult.aElem[14] = -(fFar + fNear) / (fFar - fNear);
return sResult;
}
int main(int argc, char *argv[]) {
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
glutInitWindowSize(800,800);
glutInitWindowPosition(70,70);
glutCreateWindow("ST HELENS TERRAIN");
initValues();
glutDisplayFunc(display);
glutKeyboardFunc(keyboard);
glutMouseFunc(mouse);
glutMotionFunc(mouseMotion);
glutIdleFunc(idle);
installShaders();
setCamera();
// nearPlane = radius - 1.3,farPlane = radius + 1.3;
matrixIdentity(Projection);
nearPlane = 1.3;
farPlane = 4.0;
matrixPerspective(Projection,
30, 1.0, (GLfloat) nearPlane, farPlane);
glUniform1f(nearPlaneUniform, nearPlane);
glUniform1f(farPlaneUniform, farPlane);
glClearColor(0.1,0.1,0.1,0.0);
glutMainLoop();
//freeing the memory
free(indicesArray);
free(normalsArray);
free(verticesArray);
return 0;
}
// source point clouds are assumed to contain their normals
int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residual, double pose[16])
{
int n = srcPC.rows;
const bool useRobustReject = m_rejectionScale>0;
Mat srcTemp = srcPC.clone();
Mat dstTemp = dstPC.clone();
double meanSrc[3], meanDst[3];
computeMeanCols(srcTemp, meanSrc);
computeMeanCols(dstTemp, meanDst);
double meanAvg[3]={0.5*(meanSrc[0]+meanDst[0]), 0.5*(meanSrc[1]+meanDst[1]), 0.5*(meanSrc[2]+meanDst[2])};
subtractColumns(srcTemp, meanAvg);
subtractColumns(dstTemp, meanAvg);
double distSrc = computeDistToOrigin(srcTemp);
double distDst = computeDistToOrigin(dstTemp);
double scale = (double)n / ((distSrc + distDst)*0.5);
srcTemp(cv::Range(0, srcTemp.rows), cv::Range(0,3)) *= scale;
dstTemp(cv::Range(0, dstTemp.rows), cv::Range(0,3)) *= scale;
Mat srcPC0 = srcTemp;
Mat dstPC0 = dstTemp;
// initialize pose
matrixIdentity(4, pose);
void* flann = indexPCFlann(dstPC0);
Mat M = Mat::eye(4,4,CV_64F);
double tempResidual = 0;
// walk the pyramid
for (int level = m_numLevels-1; level >=0; level--)
{
const double impact = 2;
double div = pow((double)impact, (double)level);
//double div2 = div*div;
const int numSamples = cvRound((double)(n/(div)));
const double TolP = m_tolerance*(double)(level+1)*(level+1);
const int MaxIterationsPyr = cvRound((double)m_maxIterations/(level+1));
// Obtain the sampled point clouds for this level: Also rotates the normals
Mat srcPCT = transformPCPose(srcPC0, pose);
const int sampleStep = cvRound((double)n/(double)numSamples);
std::vector<int> srcSampleInd;
/*
Note by Tolga Birdal
Downsample the model point clouds. If more optimization is required,
one could also downsample the scene points, but I think this might
decrease the accuracy. That's why I won't be implementing it at this
moment.
Also note that you have to compute a KD-tree for each level.
*/
srcPCT = samplePCUniformInd(srcPCT, sampleStep, srcSampleInd);
double fval_old=9999999999;
double fval_perc=0;
double fval_min=9999999999;
Mat Src_Moved = srcPCT.clone();
int i=0;
size_t numElSrc = (size_t)Src_Moved.rows;
int sizesResult[2] = {(int)numElSrc, 1};
float* distances = new float[numElSrc];
int* indices = new int[numElSrc];
Mat Indices(2, sizesResult, CV_32S, indices, 0);
Mat Distances(2, sizesResult, CV_32F, distances, 0);
// use robust weighting for outlier treatment
int* indicesModel = new int[numElSrc];
int* indicesScene = new int[numElSrc];
int* newI = new int[numElSrc];
int* newJ = new int[numElSrc];
double PoseX[16]={0};
matrixIdentity(4, PoseX);
while ( (!(fval_perc<(1+TolP) && fval_perc>(1-TolP))) && i<MaxIterationsPyr)
{
size_t di=0, selInd = 0;
queryPCFlann(flann, Src_Moved, Indices, Distances);
for (di=0; di<numElSrc; di++)
{
newI[di] = (int)di;
newJ[di] = indices[di];
}
if (useRobustReject)
{
int numInliers = 0;
float threshold = getRejectionThreshold(distances, Distances.rows, m_rejectionScale);
Mat acceptInd = Distances<threshold;
uchar *accPtr = (uchar*)acceptInd.data;
for (int l=0; l<acceptInd.rows; l++)
{
if (accPtr[l])
{
newI[numInliers] = l;
newJ[numInliers] = indices[l];
numInliers++;
}
}
numElSrc=numInliers;
}
// Step 2: Picky ICP
// Among the resulting corresponding pairs, if more than one scene point p_i
// is assigned to the same model point m_j, then select p_i that corresponds
// to the minimum distance
hashtable_int* duplicateTable = getHashtable(newJ, numElSrc, dstPC0.rows);
for (di=0; di<duplicateTable->size; di++)
{
hashnode_i *node = duplicateTable->nodes[di];
if (node)
{
// select the first node
int idx = reinterpret_cast<long>(node->data)-1, dn=0;
int dup = (int)node->key-1;
int minIdxD = idx;
float minDist = distances[idx];
while ( node )
{
idx = reinterpret_cast<long>(node->data)-1;
if (distances[idx] < minDist)
{
minDist = distances[idx];
minIdxD = idx;
}
node = node->next;
dn++;
}
indicesModel[ selInd ] = newI[ minIdxD ];
indicesScene[ selInd ] = dup ;
selInd++;
}
}
hashtableDestroy(duplicateTable);
if (selInd)
{
Mat Src_Match = Mat((int)selInd, srcPCT.cols, CV_64F);
Mat Dst_Match = Mat((int)selInd, srcPCT.cols, CV_64F);
for (di=0; di<selInd; di++)
{
const int indModel = indicesModel[di];
const int indScene = indicesScene[di];
const float *srcPt = (float*)&srcPCT.data[indModel*srcPCT.step];
const float *dstPt = (float*)&dstPC0.data[indScene*dstPC0.step];
double *srcMatchPt = (double*)&Src_Match.data[di*Src_Match.step];
double *dstMatchPt = (double*)&Dst_Match.data[di*Dst_Match.step];
int ci=0;
for (ci=0; ci<srcPCT.cols; ci++)
{
srcMatchPt[ci] = (double)srcPt[ci];
dstMatchPt[ci] = (double)dstPt[ci];
}
}
Mat X;
minimizePointToPlaneMetric(Src_Match, Dst_Match, X);
getTransformMat(X, PoseX);
Src_Moved = transformPCPose(srcPCT, PoseX);
double fval = cv::norm(Src_Match, Dst_Match)/(double)(Src_Moved.rows);
// Calculate change in error between iterations
fval_perc=fval/fval_old;
// Store error value
fval_old=fval;
if (fval < fval_min)
fval_min = fval;
}
else
break;
i++;
}
double TempPose[16];
matrixProduct44(PoseX, pose, TempPose);
// no need to copy the last 4 rows
for (int c=0; c<12; c++)
pose[c] = TempPose[c];
residual = tempResidual;
delete[] newI;
delete[] newJ;
delete[] indicesModel;
delete[] indicesScene;
delete[] distances;
delete[] indices;
tempResidual = fval_min;
}
// Pose(1:3, 4) = Pose(1:3, 4)./scale;
pose[3] = pose[3]/scale + meanAvg[0];
pose[7] = pose[7]/scale + meanAvg[1];
pose[11] = pose[11]/scale + meanAvg[2];
// In MATLAB this would be : Pose(1:3, 4) = Pose(1:3, 4)./scale + meanAvg' - Pose(1:3, 1:3)*meanAvg';
double Rpose[9], Cpose[3];
poseToR(pose, Rpose);
matrixProduct331(Rpose, meanAvg, Cpose);
pose[3] -= Cpose[0];
pose[7] -= Cpose[1];
pose[11] -= Cpose[2];
residual = tempResidual;
destroyFlann(flann);
return 0;
}
static void
initialize(void)
{
GLfloat light0Pos[4] =
{0.3, 0.3, 0.0, 1.0};
GLfloat matAmb[4] =
{0.01, 0.01, 0.01, 1.00};
GLfloat matDiff[4] =
{0.65, 0.65, 0.65, 1.00};
GLfloat matSpec[4] =
{0.30, 0.30, 0.30, 1.00};
GLfloat matShine = 10.0;
GLfloat eyePlaneS[] =
{1.0, 0.0, 0.0, 0.0};
GLfloat eyePlaneT[] =
{0.0, 1.0, 0.0, 0.0};
GLfloat eyePlaneR[] =
{0.0, 0.0, 1.0, 0.0};
GLfloat eyePlaneQ[] =
{0.0, 0.0, 0.0, 1.0};
int i;
/* Setup Misc. */
glClearColor(0.41, 0.41, 0.31, 0.0);
glEnable(GL_DEPTH_TEST);
/* glLineWidth(2.0);*/
glCullFace(GL_FRONT);
glEnable(GL_CULL_FACE);
glMatrixMode(GL_PROJECTION);
glFrustum(-0.5, 0.5, -0.5, 0.5, 1, 3);
glMatrixMode(GL_MODELVIEW);
glTranslatef(0, 0, -2);
matrixIdentity((GLfloat *) objectXform);
for (i = 0; i < NumTextures; i++) {
matrixIdentity((GLfloat *) textureXform[i]);
}
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(0, 1, 0, 1, -1, 1);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
glRasterPos2i(0, 0);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
/* Setup Lighting */
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, matAmb);
glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, matDiff);
glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, matSpec);
glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, matShine);
glEnable(GL_COLOR_MATERIAL);
glLightfv(GL_LIGHT0, GL_POSITION, light0Pos);
glEnable(GL_LIGHT0);
glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, GL_TRUE);
glEnable(GL_LIGHTING);
/* Setup Texture */
(*loadTexture) ();
for (i = 0; i < NumTextures; i++) {
ActiveTexture(GL_TEXTURE0_ARB + i);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
glTexGeni(GL_S, GL_TEXTURE_GEN_MODE, GL_EYE_LINEAR);
glTexGenfv(GL_S, GL_EYE_PLANE, eyePlaneS);
glTexGeni(GL_T, GL_TEXTURE_GEN_MODE, GL_EYE_LINEAR);
glTexGenfv(GL_T, GL_EYE_PLANE, eyePlaneT);
glTexGeni(GL_R, GL_TEXTURE_GEN_MODE, GL_EYE_LINEAR);
glTexGenfv(GL_R, GL_EYE_PLANE, eyePlaneR);
glTexGeni(GL_Q, GL_TEXTURE_GEN_MODE, GL_EYE_LINEAR);
glTexGenfv(GL_Q, GL_EYE_PLANE, eyePlaneQ);
}
}
