bool CFeatureExtraction::DoPCA(CvMat * pMat, CvMat * pResultMat, int nSize, int nExpectedSize)
{
printf("\nCFeatureExtraction::DoPCA in\n");
int i;
printf("DoPCA: Sort our data sets in a vector each\n");
// Sort our data sets in a vector each
CvMat ** pDataSet = new CvMat*[m_nWidth*m_nHeight];
float * pData = pMat->data.fl;
for (i=0;i<m_nWidth*m_nHeight;i++)
{
pDataSet[i] = (CvMat*) malloc(sizeof(CvMat));
cvInitMatHeader(pDataSet[i], 1, nSize, CV_32FC1, &pData[i*nSize]);
}
printf("DoPCA: Calc covariance matrix\n");
// Calc covariance matrix
CvMat* pCovMat = cvCreateMat( nSize, nSize, CV_32F );
CvMat* pMeanVec = cvCreateMat( 1, nSize, CV_32F );
cvCalcCovarMatrix( (const void **)pDataSet, m_nWidth*m_nHeight, pCovMat, pMeanVec, CV_COVAR_SCALE | CV_COVAR_NORMAL );
cvReleaseMat(&pMeanVec);
printf("DoPCA: Do the SVD decomposition\n");
// Do the SVD decomposition
CvMat* pMatW = cvCreateMat( nSize, 1, CV_32F );
CvMat* pMatV = cvCreateMat( nSize, nSize, CV_32F );
CvMat* pMatU = cvCreateMat( nSize, nSize, CV_32F );
cvSVD(pCovMat, pMatW, pMatU, pMatV, CV_SVD_MODIFY_A+CV_SVD_V_T);
cvReleaseMat(&pCovMat);
cvReleaseMat(&pMatW);
cvReleaseMat(&pMatV);
printf("DoPCA: Extract the requested number of dominant eigen vectors\n");
// Extract the requested number of dominant eigen vectors
CvMat* pEigenVecs = cvCreateMat( nSize, nExpectedSize, CV_32F );
for (i=0;i<nSize;i++)
memcpy(&pEigenVecs->data.fl[i*nExpectedSize], &pMatU->data.fl[i*nSize], nExpectedSize*sizeof(float));
printf("DoPCA: Transform to the new basis\n");
// Transform to the new basis
cvMatMul(pMat, pEigenVecs, pResultMat);
cvReleaseMat(&pMatU);
cvReleaseMat(&pEigenVecs);
for (i = 0; i < m_nHeight * m_nWidth; i++)
delete [] pDataSet[i];
delete [] pDataSet;
printf("CFeatureExtraction::DoPCA out\n");
return true;
}
CvMat* HomographyCalculationThread::calculateHomographicMatrix(double newPictureApexX[],double newPictureApexY[],double pictureApexXPosition[],double pictureApexYPosition[])
{
CvMat* mmat = cvCreateMat(3,3,CV_32FC1);
CvMat* a = cvCreateMat(POINTS*2,9,CV_32FC1);
for(int count=1;count<POINTS+1;count++) {
cvmSet(a,2*count-2,0,newPictureApexX[count-1]);
cvmSet(a,2*count-2,1,newPictureApexY[count-1]);
cvmSet(a,2*count-2,2,1);
cvmSet(a,2*count-2,3,0);
cvmSet(a,2*count-2,4,0);
cvmSet(a,2*count-2,5,0);
cvmSet(a,2*count-2,6,(-newPictureApexX[count-1]*pictureApexXPosition[count-1]));
cvmSet(a,2*count-2,7,(-pictureApexXPosition[count-1]*newPictureApexY[count-1]));
cvmSet(a,2*count-2,8,-pictureApexXPosition[count-1]);
cvmSet(a,2*count-1,0,0);
cvmSet(a,2*count-1,1,0);
cvmSet(a,2*count-1,2,0);
cvmSet(a,2*count-1,3,newPictureApexX[count-1]);
cvmSet(a,2*count-1,4,newPictureApexY[count-1]);
cvmSet(a,2*count-1,5,1);
cvmSet(a,2*count-1,6,(-newPictureApexX[count-1]*pictureApexYPosition[count-1]));
cvmSet(a,2*count-1,7,(-pictureApexYPosition[count-1]*newPictureApexY[count-1]));
cvmSet(a,2*count-1,8,-pictureApexYPosition[count-1]);
}
CvMat* U = cvCreateMat(8,8,CV_32FC1);
CvMat* D = cvCreateMat(8,9,CV_32FC1);
CvMat* V = cvCreateMat(9,9,CV_32FC1);
CvMat* V22 = cvCreateMat(3,3,CV_32FC1);
mHMatrix = cvCreateMat(3,3,CV_32FC1);
CvMat* invH = cvCreateMat(3,3,CV_32FC1);
cvSVD(a, D, U, V, CV_SVD_U_T|CV_SVD_V_T);
for(int a=0;a<3;a++) {
for(int b=0;b<3;b++) {
cvmSet(mmat,a,b,cvmGet(V,8,a*3+b));
cvmSet(V22,a,b,(1/cvmGet(V,8,4)));
}
}
cvMul(mmat,V22,mHMatrix);
cvInvert(mHMatrix,invH);
cvReleaseMat(&U);
cvReleaseMat(&D);
cvReleaseMat(&V);
cvReleaseMat(&V22);
cvReleaseMat(&a);
cvReleaseMat(&mmat);
return invH;
}
CvMat* AbstractReconstructor::compute3dHomogeneousCoordinates(CvMat* projectionMatrix1, CvMat* projectionMatrix2,
CvPoint2D32f point1, CvPoint2D32f point2)
{
CvMat* A = cvCreateMat(4, 4, CV_32FC1);
//---Linear LS Triangulation
cvmSet(A, 0, 0, (point1.x * cvmGet(projectionMatrix1, 2, 0) - cvmGet(projectionMatrix1, 0, 0)));
cvmSet(A, 0, 1, (point1.x * cvmGet(projectionMatrix1, 2, 1) - cvmGet(projectionMatrix1, 0, 1)));
cvmSet(A, 0, 2, (point1.x * cvmGet(projectionMatrix1, 2, 2) - cvmGet(projectionMatrix1, 0, 2)));
cvmSet(A, 0, 3, (point1.x * cvmGet(projectionMatrix1, 2, 3) - cvmGet(projectionMatrix1, 0, 3)));
cvmSet(A, 1, 0, (point1.y * cvmGet(projectionMatrix1, 2, 0) - cvmGet(projectionMatrix1, 1, 0)));
cvmSet(A, 1, 1, (point1.y * cvmGet(projectionMatrix1, 2, 1) - cvmGet(projectionMatrix1, 1, 1)));
cvmSet(A, 1, 2, (point1.y * cvmGet(projectionMatrix1, 2, 2) - cvmGet(projectionMatrix1, 1, 2)));
cvmSet(A, 1, 3, (point1.y * cvmGet(projectionMatrix1, 2, 3) - cvmGet(projectionMatrix1, 1, 3)));
cvmSet(A, 2, 0, (point2.x * cvmGet(projectionMatrix2, 2, 0) - cvmGet(projectionMatrix2, 0, 0)));
cvmSet(A, 2, 1, (point2.x * cvmGet(projectionMatrix2, 2, 1) - cvmGet(projectionMatrix2, 0, 1)));
cvmSet(A, 2, 2, (point2.x * cvmGet(projectionMatrix2, 2, 2) - cvmGet(projectionMatrix2, 0, 2)));
cvmSet(A, 2, 3, (point2.x * cvmGet(projectionMatrix2, 2, 3) - cvmGet(projectionMatrix2, 0, 3)));
cvmSet(A, 3, 0, (point2.y * cvmGet(projectionMatrix2, 2, 0) - cvmGet(projectionMatrix2, 1, 0)));
cvmSet(A, 3, 1, (point2.y * cvmGet(projectionMatrix2, 2, 1) - cvmGet(projectionMatrix2, 1, 1)));
cvmSet(A, 3, 2, (point2.y * cvmGet(projectionMatrix2, 2, 2) - cvmGet(projectionMatrix2, 1, 2)));
cvmSet(A, 3, 3, (point2.y * cvmGet(projectionMatrix2, 2, 3) - cvmGet(projectionMatrix2, 1, 3)));
CvMat* U = cvCreateMat(4, 4, CV_32FC1);
CvMat* W = cvCreateMat(4, 1, CV_32FC1);
CvMat* V = cvCreateMat(4, 4, CV_32FC1);
cvSVD(A, W, U, V, CV_SVD_V_T);
CvMat* x = cvCreateMat(4, 1, CV_32FC1);
cvmSet(x, 0, 0, V->data.fl[12] / V->data.fl[15]);
cvmSet(x, 1, 0, V->data.fl[13] / V->data.fl[15]);
cvmSet(x, 2, 0, V->data.fl[14] / V->data.fl[15]);
cvmSet(x, 3, 0, 1.0f);
cvReleaseMat(&W);
cvReleaseMat(&U);
cvReleaseMat(&V);
cvReleaseMat(&A);
return x;
}
void
PrincipalAxis::compute(double mx, double my, double cxx, double cxy, double cyy)
{
CV_FUNCNAME( "PrincipalAxis::compute" );
__BEGIN__;
// compute the center
this->center_.x = mx;
this->center_.y = my;
// fill in covariance matrix
cvmSet( this->covariance_, 0, 0, cxx );
cvmSet( this->covariance_, 0, 1, cxy );
cvmSet( this->covariance_, 1, 0, cxy );
cvmSet( this->covariance_, 1, 1, cyy );
// compute the eigen analysis
CV_CALL( cvSVD( this->covariance_, this->eigen_values_, this->eigen_vectors_) );
// make the necessary arrangement to have well-oriented eigen vectors
// we want the coordinates of the first axis to be (+,+) or (+,-)
// and the second vector to be oriented in the trigonometric direction
CvPoint2D64f first_axis;
first_axis.x = cvmGet(this->eigen_vectors_, 0, 0);
first_axis.y = cvmGet(this->eigen_vectors_, 1, 0);
if (first_axis.x < 0)
{
cvmSet(this->eigen_vectors_, 0, 0, - first_axis.x);
cvmSet(this->eigen_vectors_, 1, 0, - first_axis.y);
}
// the second eigen vector is orthogonal to the first (covariance is symetric)
cvmSet(this->eigen_vectors_, 0, 1, - first_axis.y);
cvmSet(this->eigen_vectors_, 1, 1, first_axis.x);
// update angle
this->angle_ = atan2(cvmGet(this->eigen_vectors_, 1, 0), cvmGet(this->eigen_vectors_, 0, 0));
__END__;
__ISL_CHECK_ERROR__;
}
/*
Calculates a planar homography from point correspondeces using the direct
linear transform. Intended for use as a ransac_xform_fn.
@param pts array of points
@param mpts array of corresponding points; each pts[i], i=0..n-1,
corresponds to mpts[i]
@param n number of points in both pts and mpts; must be at least 4
@return Returns the 3x3 planar homography matrix that transforms points
in pts to their corresponding points in mpts or NULL if fewer than 4
correspondences were provided
*/
CvMat* dlt_homog( CvPoint2D64f* pts, CvPoint2D64f* mpts, int n )
{
CvMat* H, * A, * VT, * D, h, v9;
double _h[9];
int i;
if( n < 4 )
return NULL;
/* set up matrices so we can unstack homography into h; Ah = 0 */
A = cvCreateMat( 2*n, 9, CV_64FC1 );
cvZero( A );
for( i = 0; i < n; i++ )
{
cvmSet( A, 2*i, 3, -pts[i].x );
cvmSet( A, 2*i, 4, -pts[i].y );
cvmSet( A, 2*i, 5, -1.0 );
cvmSet( A, 2*i, 6, mpts[i].y * pts[i].x );
cvmSet( A, 2*i, 7, mpts[i].y * pts[i].y );
cvmSet( A, 2*i, 8, mpts[i].y );
cvmSet( A, 2*i+1, 0, pts[i].x );
cvmSet( A, 2*i+1, 1, pts[i].y );
cvmSet( A, 2*i+1, 2, 1.0 );
cvmSet( A, 2*i+1, 6, -mpts[i].x * pts[i].x );
cvmSet( A, 2*i+1, 7, -mpts[i].x * pts[i].y );
cvmSet( A, 2*i+1, 8, -mpts[i].x );
}
D = cvCreateMat( 9, 9, CV_64FC1 );
VT = cvCreateMat( 9, 9, CV_64FC1 );
cvSVD( A, D, NULL, VT, CV_SVD_MODIFY_A + CV_SVD_V_T );
v9 = cvMat( 1, 9, CV_64FC1, NULL );
cvGetRow( VT, &v9, 8 );
h = cvMat( 1, 9, CV_64FC1, _h );
cvCopy( &v9, &h, NULL );
h = cvMat( 3, 3, CV_64FC1, _h );
H = cvCreateMat( 3, 3, CV_64FC1 );
cvConvert( &h, H );
cvReleaseMat( &A );
cvReleaseMat( &D );
cvReleaseMat( &VT );
return H;
}
// Fit a hyperplane to a set of ND points.
// Note: Input points must be in the form of an NxM matrix, where M is the dimensionality.
// This function finds the best-fit plane P, in the least-squares
// sense, between the points (X,Y,Z). The resulting plane P is described
// by the coefficient vector W, where W(1)*X + W(2)*Y +W(3)*Z = W(3), for
// (X,Y,Z) on the plane P.
void cvFitPlane(const CvMat* points, float* plane){
// Estimate geometric centroid.
int nrows = points->rows;
int ncols = points->cols;
int type = points->type;
CvMat* centroid = cvCreateMat(1, ncols, type);
cvSet(centroid, cvScalar(0));
for(int c=0; c<ncols; c++){
for(int r=0; r<nrows; r++)
centroid->data.fl[c] += points->data.fl[ncols*r+c];
centroid->data.fl[c] /= nrows;
}
// Subtract geometric centroid from each point.
CvMat* points2 = cvCreateMat(nrows, ncols, type);
for(int r=0; r<nrows; r++)
for(int c=0; c<ncols; c++)
points2->data.fl[ncols*r+c] = points->data.fl[ncols*r+c] - centroid->data.fl[c];
// Evaluate SVD of covariance matrix.
CvMat* A = cvCreateMat(ncols, ncols, type);
CvMat* W = cvCreateMat(ncols, ncols, type);
CvMat* V = cvCreateMat(ncols, ncols, type);
cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T);
cvSVD(A, W, NULL, V, CV_SVD_V_T);
// Assign plane coefficients by singular vector corresponding to smallest singular value.
plane[ncols] = 0;
for(int c=0; c<ncols; c++){
plane[c] = V->data.fl[ncols*(ncols-1)+c];
plane[ncols] += plane[c]*centroid->data.fl[c];
}
// Release allocated resources.
cvReleaseMat(¢roid);
cvReleaseMat(&points2);
cvReleaseMat(&A);
cvReleaseMat(&W);
cvReleaseMat(&V);
}
CvMat* AbstractReconstructor::compute3dHomogeneousCoordinates(vector<CvMat*>& projectionMatrix, vector<CvPoint2D32f>& points)
{
int numCam = projectionMatrix.size();
int m = 2 * numCam;
int n = 4;
CvMat* A = cvCreateMat(m, n, CV_32FC1);
for (int i = 0; i < numCam; i++) {
cvmSet(A, 2 * i, 0, (points[i].x * cvmGet(projectionMatrix[i], 2, 0) - cvmGet(projectionMatrix[i], 0, 0)));
cvmSet(A, 2 * i, 1, (points[i].x * cvmGet(projectionMatrix[i], 2, 1) - cvmGet(projectionMatrix[i], 0, 1)));
cvmSet(A, 2 * i, 2, (points[i].x * cvmGet(projectionMatrix[i], 2, 2) - cvmGet(projectionMatrix[i], 0, 2)));
cvmSet(A, 2 * i, 3, (points[i].x * cvmGet(projectionMatrix[i], 2, 3) - cvmGet(projectionMatrix[i], 0, 3)));
cvmSet(A, (2 * i) + 1, 0, (points[i].y * cvmGet(projectionMatrix[i], 2, 0) - cvmGet(projectionMatrix[i], 1, 0)));
cvmSet(A, (2 * i) + 1, 1, (points[i].y * cvmGet(projectionMatrix[i], 2, 1) - cvmGet(projectionMatrix[i], 1, 1)));
cvmSet(A, (2 * i) + 1, 2, (points[i].y * cvmGet(projectionMatrix[i], 2, 2) - cvmGet(projectionMatrix[i], 1, 2)));
cvmSet(A, (2 * i) + 1, 3, (points[i].y * cvmGet(projectionMatrix[i], 2, 3) - cvmGet(projectionMatrix[i], 1, 3)));
}
CvMat* W = cvCreateMat(n, 1, CV_32FC1);
CvMat* V = cvCreateMat(n, n, CV_32FC1);
cvSVD(A, W, NULL, V, CV_SVD_MODIFY_A | CV_SVD_V_T);
CvMat* X = cvCreateMat(n, 1, CV_32FC1);
cvmSet(X, 0, 0, V->data.fl[12] / V->data.fl[15]);
cvmSet(X, 1, 0, V->data.fl[13] / V->data.fl[15]);
cvmSet(X, 2, 0, V->data.fl[14] / V->data.fl[15]);
cvmSet(X, 3, 0, 1.0f);
cvReleaseMat(&W);
cvReleaseMat(&V);
cvReleaseMat(&A);
return X;
}
static void filterPlane(IplImage * ap_depth, std::vector<IplImage *> & a_masks, std::vector<CvPoint> & a_chain, double f)
{
const int l_num_cost_pts = 200;
float l_thres = 4;
IplImage * lp_mask = cvCreateImage(cvGetSize(ap_depth), IPL_DEPTH_8U, 1);
cvSet(lp_mask, cvRealScalar(0));
std::vector<CvPoint> l_chain_vector;
float l_chain_length = 0;
float * lp_seg_length = new float[a_chain.size()];
for (int l_i = 0; l_i < (int)a_chain.size(); ++l_i)
{
float x_diff = (float)(a_chain[(l_i + 1) % a_chain.size()].x - a_chain[l_i].x);
float y_diff = (float)(a_chain[(l_i + 1) % a_chain.size()].y - a_chain[l_i].y);
lp_seg_length[l_i] = sqrt(x_diff*x_diff + y_diff*y_diff);
l_chain_length += lp_seg_length[l_i];
}
for (int l_i = 0; l_i < (int)a_chain.size(); ++l_i)
{
if (lp_seg_length[l_i] > 0)
{
int l_cur_num = cvRound(l_num_cost_pts * lp_seg_length[l_i] / l_chain_length);
float l_cur_len = lp_seg_length[l_i] / l_cur_num;
for (int l_j = 0; l_j < l_cur_num; ++l_j)
{
float l_ratio = (l_cur_len * l_j / lp_seg_length[l_i]);
CvPoint l_pts;
l_pts.x = cvRound(l_ratio * (a_chain[(l_i + 1) % a_chain.size()].x - a_chain[l_i].x) + a_chain[l_i].x);
l_pts.y = cvRound(l_ratio * (a_chain[(l_i + 1) % a_chain.size()].y - a_chain[l_i].y) + a_chain[l_i].y);
l_chain_vector.push_back(l_pts);
}
}
}
std::vector<cv::Point3d> lp_src_3Dpts(l_chain_vector.size());
for (int l_i = 0; l_i < (int)l_chain_vector.size(); ++l_i)
{
lp_src_3Dpts[l_i].x = l_chain_vector[l_i].x;
lp_src_3Dpts[l_i].y = l_chain_vector[l_i].y;
lp_src_3Dpts[l_i].z = CV_IMAGE_ELEM(ap_depth, unsigned short, cvRound(lp_src_3Dpts[l_i].y), cvRound(lp_src_3Dpts[l_i].x));
//CV_IMAGE_ELEM(lp_mask,unsigned char,(int)lp_src_3Dpts[l_i].Y,(int)lp_src_3Dpts[l_i].X)=255;
}
//cv_show_image(lp_mask,"hallo2");
//cv::imshow("hallo2",lp_mask);
reprojectPoints(lp_src_3Dpts, lp_src_3Dpts, f);
CvMat * lp_pts = cvCreateMat((int)l_chain_vector.size(), 4, CV_32F);
CvMat * lp_v = cvCreateMat(4, 4, CV_32F);
CvMat * lp_w = cvCreateMat(4, 1, CV_32F);
for (int l_i = 0; l_i < (int)l_chain_vector.size(); ++l_i)
{
CV_MAT_ELEM(*lp_pts, float, l_i, 0) = (float)lp_src_3Dpts[l_i].x;
CV_MAT_ELEM(*lp_pts, float, l_i, 1) = (float)lp_src_3Dpts[l_i].y;
CV_MAT_ELEM(*lp_pts, float, l_i, 2) = (float)lp_src_3Dpts[l_i].z;
CV_MAT_ELEM(*lp_pts, float, l_i, 3) = 1.0f;
}
cvSVD(lp_pts, lp_w, 0, lp_v);
float l_n[4] = {CV_MAT_ELEM(*lp_v, float, 0, 3),
CV_MAT_ELEM(*lp_v, float, 1, 3),
CV_MAT_ELEM(*lp_v, float, 2, 3),
CV_MAT_ELEM(*lp_v, float, 3, 3)};
CV_IMPL void
cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr,
CvMat *rotMatr, CvMat *posVect,
CvMat *rotMatrX, CvMat *rotMatrY,
CvMat *rotMatrZ, CvPoint3D64f *eulerAngles)
{
CvMat *tmpProjMatr = 0;
CvMat *tmpMatrixD = 0;
CvMat *tmpMatrixV = 0;
CvMat *tmpMatrixM = 0;
CV_FUNCNAME("cvDecomposeProjectionMatrix");
__BEGIN__;
/* Validate parameters. */
if(projMatr == 0 || calibMatr == 0 || rotMatr == 0 || posVect == 0)
CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
if(!CV_IS_MAT(projMatr) || !CV_IS_MAT(calibMatr) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(posVect))
CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
if(projMatr->cols != 4 || projMatr->rows != 3)
CV_ERROR(CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4!");
if(calibMatr->cols != 3 || calibMatr->rows != 3 || rotMatr->cols != 3 || rotMatr->rows != 3)
CV_ERROR(CV_StsUnmatchedSizes, "Size of calibration and rotation matrices must be 3x3!");
if(posVect->cols != 1 || posVect->rows != 4)
CV_ERROR(CV_StsUnmatchedSizes, "Size of position vector must be 4x1!");
CV_CALL(tmpProjMatr = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixD = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixV = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixM = cvCreateMat(3, 3, CV_64F));
/* Compute position vector. */
cvSetZero(tmpProjMatr); // Add zero row to make matrix square.
int i, k;
for(i = 0; i < 3; i++)
for(k = 0; k < 4; k++)
cvmSet(tmpProjMatr, i, k, cvmGet(projMatr, i, k));
CV_CALL(cvSVD(tmpProjMatr, tmpMatrixD, NULL, tmpMatrixV, CV_SVD_MODIFY_A + CV_SVD_V_T));
/* Save position vector. */
for(i = 0; i < 4; i++)
cvmSet(posVect, i, 0, cvmGet(tmpMatrixV, 3, i)); // Solution is last row of V.
/* Compute calibration and rotation matrices via RQ decomposition. */
cvGetCols(projMatr, tmpMatrixM, 0, 3); // M is first square matrix of P.
assert(cvDet(tmpMatrixM) != 0.0); // So far only finite cameras could be decomposed, so M has to be nonsingular [det(M) != 0].
CV_CALL(cvRQDecomp3x3(tmpMatrixM, calibMatr, rotMatr, rotMatrX, rotMatrY, rotMatrZ, eulerAngles));
__END__;
cvReleaseMat(&tmpProjMatr);
cvReleaseMat(&tmpMatrixD);
cvReleaseMat(&tmpMatrixV);
cvReleaseMat(&tmpMatrixM);
}
static CvStatus
icvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line )
{
int i;
float w0 = 0;
float x0 = 0, y0 = 0, z0 = 0;
float x2 = 0, y2 = 0, z2 = 0, xy = 0, yz = 0, xz = 0;
float dx2, dy2, dz2, dxy, dxz, dyz;
float *v;
float n;
float det[9], evc[9], evl[3];
memset( evl, 0, 3*sizeof(evl[0]));
memset( evc, 0, 9*sizeof(evl[0]));
if( weights )
{
for( i = 0; i < count; i++ )
{
float x = points[i].x;
float y = points[i].y;
float z = points[i].z;
float w = weights[i];
x2 += x * x * w;
xy += x * y * w;
xz += x * z * w;
y2 += y * y * w;
yz += y * z * w;
z2 += z * z * w;
x0 += x * w;
y0 += y * w;
z0 += z * w;
w0 += w;
}
}
else
{
for( i = 0; i < count; i++ )
{
float x = points[i].x;
float y = points[i].y;
float z = points[i].z;
x2 += x * x;
xy += x * y;
xz += x * z;
y2 += y * y;
yz += y * z;
z2 += z * z;
x0 += x;
y0 += y;
z0 += z;
}
w0 = (float) count;
}
x2 /= w0;
xy /= w0;
xz /= w0;
y2 /= w0;
yz /= w0;
z2 /= w0;
x0 /= w0;
y0 /= w0;
z0 /= w0;
dx2 = x2 - x0 * x0;
dxy = xy - x0 * y0;
dxz = xz - x0 * z0;
dy2 = y2 - y0 * y0;
dyz = yz - y0 * z0;
dz2 = z2 - z0 * z0;
det[0] = dz2 + dy2;
det[1] = -dxy;
det[2] = -dxz;
det[3] = det[1];
det[4] = dx2 + dz2;
det[5] = -dyz;
det[6] = det[2];
det[7] = det[5];
det[8] = dy2 + dx2;
/* Searching for a eigenvector of det corresponding to the minimal eigenvalue */
#if 1
{
CvMat _det = cvMat( 3, 3, CV_32F, det );
CvMat _evc = cvMat( 3, 3, CV_32F, evc );
CvMat _evl = cvMat( 3, 1, CV_32F, evl );
cvEigenVV( &_det, &_evc, &_evl, 0 );
i = evl[0] < evl[1] ? (evl[0] < evl[2] ? 0 : 2) : (evl[1] < evl[2] ? 1 : 2);
}
#else
{
CvMat _det = cvMat( 3, 3, CV_32F, det );
CvMat _evc = cvMat( 3, 3, CV_32F, evc );
CvMat _evl = cvMat( 1, 3, CV_32F, evl );
cvSVD( &_det, &_evl, &_evc, 0, CV_SVD_MODIFY_A+CV_SVD_U_T );
}
i = 2;
#endif
v = &evc[i * 3];
n = (float) sqrt( (double)v[0] * v[0] + (double)v[1] * v[1] + (double)v[2] * v[2] );
n = (float)MAX(n, eps);
line[0] = v[0] / n;
line[1] = v[1] / n;
line[2] = v[2] / n;
line[3] = x0;
line[4] = y0;
line[5] = z0;
return CV_NO_ERR;
}
// finds perspective transformation H between
// the object plane and image plane,
// so that (sxi,syi,s) ~ H*(Xi,Yi,1)
void icvFindHomography( const CvMat* object_points,
const CvMat* image_points, CvMat* __H )
{
CvMat *_m = 0, *_M = 0;
CvMat *_L2 = 0;
CV_FUNCNAME( "cvFindHomography" );
__BEGIN__;
int h_type;
int i, k, count, count2;
CvPoint2D64f *m, *M;
CvPoint2D64f cm = {0,0}, sm = {0,0};
double inv_Hnorm[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
double H[9];
CvMat _inv_Hnorm = cvMat( 3, 3, CV_64FC1, inv_Hnorm );
CvMat _H = cvMat( 3, 3, CV_64FC1, H );
double LtL[9*9], LW[9], LV[9*9];
CvMat* _Lp;
double* L;
CvMat _LtL = cvMat( 9, 9, CV_64FC1, LtL );
CvMat _LW = cvMat( 9, 1, CV_64FC1, LW );
CvMat _LV = cvMat( 9, 9, CV_64FC1, LV );
CvMat _Hrem = cvMat( 3, 3, CV_64FC1, LV + 8*9 );
if( !CV_IS_MAT(image_points) || !CV_IS_MAT(object_points) ||
!CV_IS_MAT(__H) )
CV_ERROR( CV_StsBadArg, "one of arguments is not a valid matrix" );
h_type = CV_MAT_TYPE(__H->type);
if( h_type != CV_32FC1 && h_type != CV_64FC1 )
CV_ERROR( CV_StsUnsupportedFormat,
"Homography matrix must have 32fC1 or 64fC1 type" );
if( __H->rows != 3 || __H->cols != 3 )
CV_ERROR( CV_StsBadSize, "Homography matrix must be 3x3" );
count = MAX(image_points->cols, image_points->rows);
count2 = MAX(object_points->cols, object_points->rows);
if( count != count2 )
CV_ERROR( CV_StsUnmatchedSizes,
"Numbers of image and object points do not match" );
CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( icvConvertPointsHomogenious( image_points, _m ));
m = (CvPoint2D64f*)_m->data.ptr;
CV_CALL( _M = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( icvConvertPointsHomogenious( object_points, _M ));
M = (CvPoint2D64f*)_M->data.ptr;
// calculate the normalization transformation Hnorm.
for( i = 0; i < count; i++ )
cm.x += m[i].x, cm.y += m[i].y;
cm.x /= count; cm.y /= count;
for( i = 0; i < count; i++ )
{
double x = m[i].x - cm.x;
double y = m[i].y - cm.y;
sm.x += fabs(x); sm.y += fabs(y);
}
sm.x /= count; sm.y /= count;
inv_Hnorm[0] = sm.x;
inv_Hnorm[4] = sm.y;
inv_Hnorm[2] = cm.x;
inv_Hnorm[5] = cm.y;
sm.x = 1./sm.x;
sm.y = 1./sm.y;
CV_CALL( _Lp = _L2 = cvCreateMat( 2*count, 9, CV_64FC1 ) );
L = _L2->data.db;
for( i = 0; i < count; i++, L += 18 )
{
double x = -(m[i].x - cm.x)*sm.x, y = -(m[i].y - cm.y)*sm.y;
L[0] = L[9 + 3] = M[i].x;
L[1] = L[9 + 4] = M[i].y;
L[2] = L[9 + 5] = 1;
L[9 + 0] = L[9 + 1] = L[9 + 2] = L[3] = L[4] = L[5] = 0;
L[6] = x*M[i].x;
L[7] = x*M[i].y;
L[8] = x;
L[9 + 6] = y*M[i].x;
L[9 + 7] = y*M[i].y;
L[9 + 8] = y;
}
if( count > 4 )
{
cvMulTransposed( _L2, &_LtL, 1 );
_Lp = &_LtL;
}
_LW.rows = MIN(count*2, 9);
cvSVD( _Lp, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
cvScale( &_Hrem, &_Hrem, 1./_Hrem.data.db[8] );
cvMatMul( &_inv_Hnorm, &_Hrem, &_H );
if( count > 4 )
{
// reuse the available storage for jacobian and other vars
CvMat _J = cvMat( 2*count, 8, CV_64FC1, _L2->data.db );
CvMat _err = cvMat( 2*count, 1, CV_64FC1, _L2->data.db + 2*count*8 );
CvMat _JtJ = cvMat( 8, 8, CV_64FC1, LtL );
CvMat _JtErr = cvMat( 8, 1, CV_64FC1, LtL + 8*8 );
CvMat _JtJW = cvMat( 8, 1, CV_64FC1, LW );
CvMat _JtJV = cvMat( 8, 8, CV_64FC1, LV );
CvMat _Hinnov = cvMat( 8, 1, CV_64FC1, LV + 8*8 );
for( k = 0; k < 10; k++ )
{
double* J = _J.data.db, *err = _err.data.db;
for( i = 0; i < count; i++, J += 16, err += 2 )
{
double di = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);
double _xi = (H[0]*M[i].x + H[1]*M[i].y + H[2])*di;
double _yi = (H[3]*M[i].x + H[4]*M[i].y + H[5])*di;
err[0] = m[i].x - _xi;
err[1] = m[i].y - _yi;
J[0] = M[i].x*di;
J[1] = M[i].y*di;
J[2] = di;
J[8+3] = M[i].x;
J[8+4] = M[i].y;
J[8+5] = di;
J[6] = -J[0]*_xi;
J[7] = -J[1]*_xi;
J[8+6] = -J[8+3]*_yi;
J[8+7] = -J[8+4]*_yi;
J[3] = J[4] = J[5] = J[8+0] = J[8+1] = J[8+2] = 0.;
}
icvGaussNewton( &_J, &_err, &_Hinnov, &_JtJ, &_JtErr, &_JtJW, &_JtJV );
for( i = 0; i < 8; i++ )
H[i] += _Hinnov.data.db[i];
}
}
cvConvert( &_H, __H );
__END__;
cvReleaseMat( &_m );
cvReleaseMat( &_M );
cvReleaseMat( &_L2 );
}
void icvGaussNewton( const CvMat* J, const CvMat* err, CvMat* delta,
CvMat* JtJ, CvMat* JtErr, CvMat* JtJW, CvMat* JtJV )
{
CvMat* _temp_JtJ = 0;
CvMat* _temp_JtErr = 0;
CvMat* _temp_JtJW = 0;
CvMat* _temp_JtJV = 0;
CV_FUNCNAME( "icvGaussNewton" );
__BEGIN__;
if( !CV_IS_MAT(J) || !CV_IS_MAT(err) || !CV_IS_MAT(delta) )
CV_ERROR( CV_StsBadArg,
"Some of required arguments is not a valid matrix" );
if( !JtJ )
{
CV_CALL( _temp_JtJ = cvCreateMat( J->cols, J->cols, J->type ));
JtJ = _temp_JtJ;
}
else if( !CV_IS_MAT(JtJ) )
CV_ERROR( CV_StsBadArg, "JtJ is not a valid matrix" );
if( !JtErr )
{
CV_CALL( _temp_JtErr = cvCreateMat( J->cols, 1, J->type ));
JtErr = _temp_JtErr;
}
else if( !CV_IS_MAT(JtErr) )
CV_ERROR( CV_StsBadArg, "JtErr is not a valid matrix" );
if( !JtJW )
{
CV_CALL( _temp_JtJW = cvCreateMat( J->cols, 1, J->type ));
JtJW = _temp_JtJW;
}
else if( !CV_IS_MAT(JtJW) )
CV_ERROR( CV_StsBadArg, "JtJW is not a valid matrix" );
if( !JtJV )
{
CV_CALL( _temp_JtJV = cvCreateMat( J->cols, J->cols, J->type ));
JtJV = _temp_JtJV;
}
else if( !CV_IS_MAT(JtJV) )
CV_ERROR( CV_StsBadArg, "JtJV is not a valid matrix" );
cvMulTransposed( J, JtJ, 1 );
cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
cvSVD( JtJ, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
cvSVBkSb( JtJW, JtJV, JtJV, JtErr, delta, CV_SVD_U_T + CV_SVD_V_T );
__END__;
if( _temp_JtJ || _temp_JtErr || _temp_JtJW || _temp_JtJV )
{
cvReleaseMat( &_temp_JtJ );
cvReleaseMat( &_temp_JtErr );
cvReleaseMat( &_temp_JtJW );
cvReleaseMat( &_temp_JtJV );
}
}
CvMat* vgg_X_from_xP_nonlin(CvMat* u1, CvMat** P1, CvMat* imsize1, int K)
{
CvMat* u;
CvMat** P=new CvMat* [K];
CvMat* imsize;
u=cvCreateMat(u1->rows,u1->cols,u1->type);
cvCopy(u1,u);
int kp;
for(kp=0;kp<K;kp++)
{
P[kp]=cvCreateMat(P1[kp]->rows,P1[kp]->cols,P1[kp]->type);
cvCopy(P1[kp],P[kp]);
}
imsize=cvCreateMat(imsize1->rows,imsize1->cols,imsize1->type);
cvCopy(imsize1,imsize);
CvMat* X;
CvMat* H;
CvMat* u_2_rows;
CvMat* W;
CvMat* U;
CvMat* T;
CvMat* Y;
CvMat** Q=new CvMat*[K];//Q is a variable not well defined
CvMat* J;
CvMat* e;
CvMat* J_tr;
CvMat* eprev;
CvMat* JtJ;
CvMat* Je;
CvMat* Y_new;
CvMat* T_2_cols;
CvMat* T_rest_cols;
CvMat* X_T;
CvScalar f, inf;
int i, mat_id;
int u_rows = u->rows;
int u_cols = u->cols;
double lambda_min, lambda_max;
CvMat* imsize_col = cvCreateMat(2, 1, CV_64FC1);
/* K is the number of images */
if(K < 2)
{
printf("\n Cannot reconstruct 3D from 1 image");
return 0;
}
/* Create temporary matrix for the linear function */
u_2_rows = cvCreateMat(2, u_cols, CV_64FC1);
/* Initialize the temporary matrix by extracting the first two rows */
u_2_rows = cvGetRows(u, u_2_rows, 0, 2, 1);
/* Call the linear function */
X = vgg_X_from_xP_lin(u_2_rows, P, K, imsize);
imsize_col = cvGetCol(imsize, imsize_col, 0);
f = cvSum(imsize_col);
f.val[0] = 4 / f.val[0];
/* Create and initialize H matrix */
H = cvCreateMat(3, 3, CV_64FC1);
H->data.db[0] = f.val[0];
H->data.db[1] = 0;
H->data.db[2] = ((-1) * f.val[0] * cvmGet(imsize, 0, 0)) / 2;
H->data.db[3] = 0;
H->data.db[4] = f.val[0];
H->data.db[5] = ((-1) * f.val[0] * cvmGet(imsize, 1, 0)) / 2;
H->data.db[6] = 0;
H->data.db[7] = 0;
H->data.db[8] = 1;
for(mat_id = 0; mat_id < K ; mat_id++)
{
cvMatMul(H, P[mat_id], P[mat_id]);
}
/* u = H * u; */
cvMatMul(H, u, u);
/*
//debug
printf("....H\n");
CvMat_printdb(stdout,"%7.3f ",H);
//debug
printf("....u\n");
CvMat_printdb(stdout,"%7.3f ",u);
*/
/* Parametrize X such that X = T*[Y;1]; thus x = P*T*[Y;1] = Q*[Y;1] */
/* Create the SVD matrices X = U*W*V'*/
X_T = cvCreateMat(X->cols, X->rows, CV_64FC1); /* M * N */
W = cvCreateMat(X_T->rows, X_T->cols, CV_64FC1); /* M * N */
U = cvCreateMat(X_T->rows, X_T->rows, CV_64FC1); /* M * M */
T = cvCreateMat(X_T->cols, X_T->cols, CV_64FC1); /* N * N */
cvTranspose(X, X_T);
cvSVD(X_T, W, U, T);
cvReleaseMat(&W);
/* T = T(:,[2:end 1]); */
/* Initialize the temporary matrix by extracting the first two columns */
/* Create temporary matrix for the linear function */
T_2_cols = cvCreateMat(T->rows, 2, CV_64FC1);
T_rest_cols = cvCreateMat(T->rows, (T->cols - 2), CV_64FC1);
/* Initialize the temporary matrix by extracting the first two columns */
T_2_cols= sfmGetCols(T,0,0);
T_rest_cols=sfmGetCols(T,1,T->cols-1);
T=sfmAlignMatH(T_rest_cols,T_2_cols);
for(mat_id = 0; mat_id < K ; mat_id++)
{
/* Create temporary matrix for the linear function */
Q[mat_id] = cvCreateMat(P[mat_id]->rows, T->cols, CV_64FC1);
cvMatMul(P[mat_id], T, Q[mat_id]);
}
/*
//debug
printf("....Q0\n");
CvMat_printdb(stdout,"%7.3f ",Q[0]);
//debug
printf("....Q1\n");
CvMat_printdb(stdout,"%7.3f ",Q[1]);
*/
/* Newton estimation */
/* Create the required Y matrix for the Newton process */
Y = cvCreateMat(3, 1, CV_64FC1);
cvSetZero(Y); /* Y = [0;0;0] */
/* Initialize the infinite array */
inf.val[0] = INF;
inf.val[1] = INF;
inf.val[2] = INF;
inf.val[3] = INF;
eprev = cvCreateMat(1, 1, CV_64FC1);
cvSet(eprev, inf, 0);
for(i = 0 ; i < 10 ; i++)
{
// printf("i=%d....\n",i);
int pass;
double RCondVal;
//initialize e,J before using.
e=cvCreateMat(2*K,1,CV_64FC1);
J=cvCreateMat(2*K,3,CV_64FC1);
pass = resid(Y, u, Q, K, e, J);
J_tr = cvCreateMat(J->cols, J->rows, CV_64FC1);
cvTranspose(J, J_tr);
JtJ = cvCreateMat(J->cols, J->cols, CV_64FC1);
cvMatMul(J_tr, J, JtJ);
//prevent memory leak;
cvReleaseMat(&W);
/* Create the SVD matrices JtJ = U*W*V'*/
W = cvCreateMat(J->cols, J->cols, CV_64FC1);
cvSVD(JtJ, W);
/*
//debug
printf("....W\n");
CvMat_printdb(stdout,"%7.3f ",W);
*/
lambda_max = W->data.db[0];
lambda_min = W->data.db[((W->rows * W->cols) - 1)];
RCondVal = lambda_min / lambda_max;
if(1 - (cvNorm(e, 0, CV_L2, 0) / cvNorm(eprev, 0, CV_L2, 0)) < 1000 * EPS)
{
cvReleaseMat(&J);
cvReleaseMat(&e);
cvReleaseMat(&J_tr);
cvReleaseMat(&JtJ);
cvReleaseMat(&W);
break;
}
if(RCondVal < 10 * EPS)
{
cvReleaseMat(&J);
cvReleaseMat(&e);
cvReleaseMat(&J_tr);
cvReleaseMat(&JtJ);
cvReleaseMat(&W);
break;
}
cvReleaseMat(&eprev);
eprev = cvCreateMat(e->rows, e->cols, CV_64FC1);
cvCopy(e, eprev);
Je = cvCreateMat(J->cols, e->cols, CV_64FC1);
cvMatMul(J_tr, e, Je); /* (J'*e) */
/*
//debug
printf("....J_tr\n");
CvMat_printdb(stdout,"%7.3f ",J_tr);
//debug
printf("....e\n");
CvMat_printdb(stdout,"%7.3f ",e);
//debug
printf("....JtJ\n");
CvMat_printdb(stdout,"%7.3f ",JtJ);
//debug
printf("....Je\n");
CvMat_printdb(stdout,"%7.3f ",Je);
//debug
printf("....JtJ\n");
CvMat_printdb(stdout,"%7.3f ",JtJ);
*/
cvInvert(JtJ,JtJ);
/* (J'*J)\(J'*e) */
Je=sfmMatMul(JtJ, Je);
/*
//debug
printf("....Je\n");
CvMat_printdb(stdout,"%7.3f ",Je);
*/
/* Y = Y - (J'*J)\(J'*e) */
cvSub(Y, Je, Y, 0);
/*
//debug
printf("....Y\n");
CvMat_printdb(stdout,"%7.3f ",Y);
*/
cvReleaseMat(&J);
cvReleaseMat(&e);
cvReleaseMat(&J_tr);
cvReleaseMat(&JtJ);
cvReleaseMat(&Je);
cvReleaseMat(&W);
}
Y_new = cvCreateMat(4, 1, CV_64FC1);
PutMatV(Y,Y_new,0);
Y_new->data.db[3]=1;
/*
//debug
printf("....Y_new\n");
CvMat_printdb(stdout,"%7.3f ",Y_new);
printf("....T\n");
CvMat_printdb(stdout,"%7.3f ",T);
*/
/* Obtain the new X */
cvMatMul(T, Y_new, X);
cvReleaseMat(&H);
cvReleaseMat(&u_2_rows);
cvReleaseMat(&U);
cvReleaseMat(&T);
cvReleaseMat(&Y);
cvReleaseMat(&Y_new);
cvReleaseMat(&T_2_cols);
cvReleaseMat(&T_rest_cols);
for(kp=0;kp<K;kp++)
{
cvReleaseMat(&P[kp]);
}
cvReleaseMat(&u);
cvReleaseMat(&imsize);
cvReleaseMatGrp(Q,K);
return X;
}
//!Estimate transform from a set of points
void SimilarityTransform::estimateFromPoints(const CvMat * points1, const CvMat * points2)
{
//const CvMat * temp;
//CV_SWAP(points1, points2, temp);
/* AffineTransform * pAT = getAffineTransform();
pAT->estimateFromPoints(points1, points2);
delete pAT;*/
//Umeyama's algorithm:
//Find mean and s.d.
int numPoints = points1->cols;
double meanP1Data[2];
CvMat meanP1 = cvMat(2, 1, CV_64FC1, meanP1Data);
double meanP2Data[2];
CvMat meanP2 = cvMat(2, 1, CV_64FC1, meanP2Data);
cvSetZero(&meanP1);
cvSetZero(&meanP2);
for(int i = 0; i<numPoints; i++)
{
meanP1Data[0] += cvmGet(points1, 0, i);
meanP1Data[1] += cvmGet(points1, 1, i);
meanP2Data[0] += cvmGet(points2, 0, i);
meanP2Data[1] += cvmGet(points2, 1, i);
}
double numPoints_inv = 1.0/numPoints;
meanP1Data[0] *= numPoints_inv;
meanP1Data[1] *= numPoints_inv;
meanP2Data[0] *= numPoints_inv;
meanP2Data[1] *= numPoints_inv;
//Now calculate variance
double varP1, varP2;
varP1 = 0;
varP2 = 0;
double SIGMAData[4];
CvMat SIGMA = cvMat(2, 2, CV_64FC1, SIGMAData);
cvSetZero(&SIGMA);
for(int i = 0; i<numPoints; i++)
{
double x1 = cvmGet(points1, 0, i) - meanP1Data[0];
double y1 = cvmGet(points1, 1, i) - meanP1Data[1];
double x2 = cvmGet(points2, 0, i) - meanP2Data[0];
double y2 = cvmGet(points2, 1, i) - meanP2Data[1];
varP1 += sqr(x1) + sqr(y1);
varP2 += sqr(x2) + sqr(y2);
SIGMAData[0] += x1*x2;
SIGMAData[1] += x1*y2;
SIGMAData[2] += y1*x2;
SIGMAData[3] += y1*y2;
}
varP1 *= numPoints_inv;
varP2 *= numPoints_inv;
SIGMAData[0] *= numPoints_inv;
SIGMAData[1] *= numPoints_inv;
SIGMAData[2] *= numPoints_inv;
SIGMAData[3] *= numPoints_inv;
double DData[4];
CvMat D = cvMat(2, 2, CV_64FC1, DData);
cvSetZero(&D);
double UData[4];
CvMat U = cvMat(2, 2, CV_64FC1, UData);
cvSetZero(&U);
double VData[4];
CvMat V = cvMat(2, 2, CV_64FC1, VData);
cvSetZero(&V);
double RotationData[4];
CvMat Rotation = cvMat(2, 2, CV_64FC1, RotationData);
cvSetZero(&Rotation);
cvSVD(&SIGMA, &D, &U, &V);
cvGEMM(&U, &V, 1, 0, 0, &Rotation, CV_GEMM_B_T);
// theta_ = acos(RotationData[0]);
theta_ = asin(RotationData[1]);
scale_ = (1.0/varP1) * cvTrace(&D).val[0];
double transData[2];
CvMat trans = cvMat(2, 1, CV_64FC1, transData);
cvSetZero(&trans);
cvGEMM( &Rotation, &meanP1, -scale_, &meanP2, 1, &trans);
translation_ = cvPoint2D64f(transData[0], transData[1]);
//Applying this to p1 gives us p2
}
/* for now this function works bad with singular cases
You can see in the code, that when some troubles with
matrices or some variables occur -
box filled with zero values is returned.
However in general function works fine.
*/
static void
icvFitEllipse_F( CvSeq* points, CvBox2D* box )
{
CvMat* D = 0;
CV_FUNCNAME( "icvFitEllipse_F" );
__BEGIN__;
double S[36], C[36], T[36];
int i, j;
double eigenvalues[6], eigenvectors[36];
double a, b, c, d, e, f;
double x0, y0, idet, scale, offx = 0, offy = 0;
int n = points->total;
CvSeqReader reader;
int is_float = CV_SEQ_ELTYPE(points) == CV_32FC2;
CvMat _S = cvMat(6,6,CV_64F,S), _C = cvMat(6,6,CV_64F,C), _T = cvMat(6,6,CV_64F,T);
CvMat _EIGVECS = cvMat(6,6,CV_64F,eigenvectors), _EIGVALS = cvMat(6,1,CV_64F,eigenvalues);
/* create matrix D of input points */
CV_CALL( D = cvCreateMat( n, 6, CV_64F ));
cvStartReadSeq( points, &reader );
/* shift all points to zero */
for( i = 0; i < n; i++ )
{
if( !is_float )
{
offx += ((CvPoint*)reader.ptr)->x;
offy += ((CvPoint*)reader.ptr)->y;
}
else
{
offx += ((CvPoint2D32f*)reader.ptr)->x;
offy += ((CvPoint2D32f*)reader.ptr)->y;
}
CV_NEXT_SEQ_ELEM( points->elem_size, reader );
}
offx /= n;
offy /= n;
// fill matrix rows as (x*x, x*y, y*y, x, y, 1 )
for( i = 0; i < n; i++ )
{
double x, y;
double* Dptr = D->data.db + i*6;
if( !is_float )
{
x = ((CvPoint*)reader.ptr)->x - offx;
y = ((CvPoint*)reader.ptr)->y - offy;
}
else
{
x = ((CvPoint2D32f*)reader.ptr)->x - offx;
y = ((CvPoint2D32f*)reader.ptr)->y - offy;
}
CV_NEXT_SEQ_ELEM( points->elem_size, reader );
Dptr[0] = x * x;
Dptr[1] = x * y;
Dptr[2] = y * y;
Dptr[3] = x;
Dptr[4] = y;
Dptr[5] = 1.;
}
// S = D^t*D
cvMulTransposed( D, &_S, 1 );
cvSVD( &_S, &_EIGVALS, &_EIGVECS, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
for( i = 0; i < 6; i++ )
{
double a = eigenvalues[i];
a = a < DBL_EPSILON ? 0 : 1./sqrt(sqrt(a));
for( j = 0; j < 6; j++ )
eigenvectors[i*6 + j] *= a;
}
// C = Q^-1 = transp(INVEIGV) * INVEIGV
cvMulTransposed( &_EIGVECS, &_C, 1 );
cvZero( &_S );
S[2] = 2.;
S[7] = -1.;
S[12] = 2.;
// S = Q^-1*S*Q^-1
cvMatMul( &_C, &_S, &_T );
cvMatMul( &_T, &_C, &_S );
// and find its eigenvalues and vectors too
//cvSVD( &_S, &_EIGVALS, &_EIGVECS, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 );
for( i = 0; i < 3; i++ )
if( eigenvalues[i] > 0 )
break;
if( i >= 3 /*eigenvalues[0] < DBL_EPSILON*/ )
{
box->center.x = box->center.y =
box->size.width = box->size.height =
box->angle = 0.f;
EXIT;
}
// now find truthful eigenvector
_EIGVECS = cvMat( 6, 1, CV_64F, eigenvectors + 6*i );
_T = cvMat( 6, 1, CV_64F, T );
// Q^-1*eigenvecs[0]
cvMatMul( &_C, &_EIGVECS, &_T );
// extract vector components
a = T[0]; b = T[1]; c = T[2]; d = T[3]; e = T[4]; f = T[5];
///////////////// extract ellipse axes from above values ////////////////
/*
1) find center of ellipse
it satisfy equation
| a b/2 | * | x0 | + | d/2 | = |0 |
| b/2 c | | y0 | | e/2 | |0 |
*/
idet = a * c - b * b * 0.25;
idet = idet > DBL_EPSILON ? 1./idet : 0;
// we must normalize (a b c d e f ) to fit (4ac-b^2=1)
scale = sqrt( 0.25 * idet );
if( scale < DBL_EPSILON )
{
box->center.x = (float)offx;
box->center.y = (float)offy;
box->size.width = box->size.height = box->angle = 0.f;
EXIT;
}
a *= scale;
b *= scale;
c *= scale;
d *= scale;
e *= scale;
f *= scale;
x0 = (-d * c + e * b * 0.5) * 2.;
y0 = (-a * e + d * b * 0.5) * 2.;
// recover center
box->center.x = (float)(x0 + offx);
box->center.y = (float)(y0 + offy);
// offset ellipse to (x0,y0)
// new f == F(x0,y0)
f += a * x0 * x0 + b * x0 * y0 + c * y0 * y0 + d * x0 + e * y0;
if( fabs(f) < DBL_EPSILON )
{
box->size.width = box->size.height = box->angle = 0.f;
EXIT;
}
scale = -1. / f;
// normalize to f = 1
a *= scale;
b *= scale;
c *= scale;
// extract axis of ellipse
// one more eigenvalue operation
S[0] = a;
S[1] = S[2] = b * 0.5;
S[3] = c;
_S = cvMat( 2, 2, CV_64F, S );
_EIGVECS = cvMat( 2, 2, CV_64F, eigenvectors );
_EIGVALS = cvMat( 1, 2, CV_64F, eigenvalues );
cvSVD( &_S, &_EIGVALS, &_EIGVECS, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
// exteract axis length from eigenvectors
box->size.width = (float)(2./sqrt(eigenvalues[0]));
box->size.height = (float)(2./sqrt(eigenvalues[1]));
// calc angle
box->angle = (float)(180 - atan2(eigenvectors[2], eigenvectors[3])*180/CV_PI);
__END__;
cvReleaseMat( &D );
}
CvMat* CreateHomographyRotationTranslationMatrix( CvMat* m_homography,CvMat* m_intric )
{
//http://forum.openframeworks.cc/index.php?topic=2313.0
//This produces Relative Rotation
// Suppose you are at a some position P(X,Y,Z), and you are looking off in some direction D. Your position is represented by the translation matrix T, and the direction of your view is represented by the rotation matrix R. The combined information is held in the translation matrix Tr. We saw this at the beginning of the presentation:
int i;
// Vectors holding columns of H and R:
//Homography cvGetCol().
double a_H1[3];
CvMat m_H1 = cvMat( 3, 1, CV_64FC1, a_H1 );
for( i = 0; i < 3; i++ ) cvmSet( &m_H1, i, 0, cvmGet( m_homography, i, 0 ) );
double a_H2[3];
CvMat m_H2 = cvMat( 3, 1, CV_64FC1, a_H2 );
for( i = 0; i < 3; i++ ) cvmSet( &m_H2, i, 0, cvmGet( m_homography, i, 1 ) );
double a_H3[3];
CvMat m_H3 = cvMat( 3, 1, CV_64FC1, a_H3 );
for( i = 0; i < 3; i++ ) cvmSet( &m_H3, i, 0, cvmGet( m_homography, i, 2 ) );
double a_CinvH1[3];
CvMat m_CinvH1 = cvMat( 3, 1, CV_64FC1, a_CinvH1 );
double a_R1[3];
CvMat m_R1 = cvMat( 3, 1, CV_64FC1, a_R1 );
double a_R2[3];
CvMat m_R2 = cvMat( 3, 1, CV_64FC1, a_R2 );
double a_R3[3];
CvMat m_R3 = cvMat( 3, 1, CV_64FC1, a_R3 );
// The rotation matrix:
double a_R[9];
CvMat m_R = cvMat( 3, 3, CV_64FC1, a_R );
// The translation vector:
double a_T[3];
CvMat m_T = cvMat( 3, 1, CV_64FC1, a_T );
////////////////////////////////////////////////////////
// Create inverse calibration matrix:
CvMat* m_Cinv = inverseCalibrationMatrix( m_intric);// .
// Create norming factor lambda:
cvGEMM( m_Cinv, &m_H1, 1, NULL, 0, &m_CinvH1, 0 );
// Search next orthonormal matrix:
if( cvNorm( &m_CinvH1, NULL, CV_L2, NULL ) != 0 )
{
double lambda = 1.00/cvNorm( &m_CinvH1, NULL, CV_L2, NULL );
// Create normalized R1 & R2:
cvGEMM( m_Cinv, &m_H1, lambda, NULL, 0, &m_R1, 0 );
cvGEMM( m_Cinv, &m_H2, lambda, NULL, 0, &m_R2, 0 );
// Get R3 orthonormal to R1 and R2:
cvCrossProduct( &m_R1, &m_R2, &m_R3 );
// Put the rotation column vectors in the rotation matrix:
for( i = 0; i < 3; i++ ){
cvmSet( &m_R, i, 0, cvmGet( &m_R1, i, 0 ) );
cvmSet( &m_R, i, 1, cvmGet( &m_R2, i, 0 ) );
cvmSet( &m_R, i, 2, cvmGet( &m_R3, i, 0 ) );
}
// Calculate Translation Vector T (- because of its definition):
cvGEMM( m_Cinv, &m_H3, -lambda, NULL, 0, &m_T, 0 );
// Transformation of R into - in Frobenius sense - next orthonormal matrix:
double a_W[9]; CvMat m_W = cvMat( 3, 3, CV_64FC1, a_W );
double a_U[9]; CvMat m_U = cvMat( 3, 3, CV_64FC1, a_U );
double a_Vt[9]; CvMat m_Vt = cvMat( 3, 3, CV_64FC1, a_Vt );
cvSVD( &m_R, &m_W, &m_U, &m_Vt, CV_SVD_MODIFY_A | CV_SVD_V_T );
cvMatMul( &m_U, &m_Vt, &m_R );
// Put the rotation matrix and the translation vector together:
CvMat* m_view_to_cam = cvCreateMat( 3, 4, CV_64FC1 );
ComposeRotationTranslationTo3x4Matrix( m_view_to_cam, &m_R, &m_T );
return m_view_to_cam;
}
return 0;
}
int CUKF::UnscentedTransform(int model)
{
// Set up some variables
int dim = cvGetSize(XX).height;
int N = 2*dim+1;
double scale = 3;
int kappa = scale-dim;
// Create samples
// SVD
CvSize P = cvGetSize(PX);
CvMat *D = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *U = cvCreateMat(P.height, P.height, CV_64FC1);
CvMat *V = cvCreateMat(P.width, P.width, CV_64FC1);
CvMat *Ds = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *UD = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *Ps = cvCreateMat(P.height, P.width, CV_64FC1);
cvSVD(PX, D, U, V, CV_SVD_V_T);
MatSqrt(D, Ds);
cvMatMul(U, Ds, UD);
cvScale(UD, Ps, sqrt(scale), 0);
// PrintCvMat(U, "U");
// PrintCvMat(D, "D");
// PrintCvMat(V, "V");
// PrintCvMat(Ds, "Ds");
// PrintCvMat(UD, "UD");
// PrintCvMat(PX, "PX");
// PrintCvMat(Ps, "Ps");
CvMat *ss = cvCreateMat(dim, N, CV_64FC1);
//for sigma points display
//cvReleaseMat(&m_sigmaPointst_1);
//cvReleaseMat(&m_sigmaPointst);
//m_sigmaPointst_1 = cvCreateMat(dim, N, CV_64FC1);
//m_sigmaPointst = cvCreateMat(dim, N, CV_64FC1);
//
int i,j,jj;
int row = dim;
int col = N;
cvRepeat(XX, ss);
double* ssdb;
double* Psdb;
ssdb = ss->data.db;
Psdb = Ps->data.db;
for(i=0; i<row; i++)
{
for(j=1, jj=0; j<dim+1; j++, jj++)
{
//ss->data.db[i*ss->cols+j] += Ps->data.db[i*Ps->cols+jj];
ssdb[i*ss->cols+j] += Psdb[i*Ps->cols+jj];
}
}
for(i=0; i<row; i++)
{
for(j=1+dim, jj=0; j<ss->cols; j++, jj++)
{
//ss->data.db[i*ss->cols+j] -= Ps->data.db[i*Ps->cols+jj];
ssdb[i*ss->cols+j] -= Psdb[i*Ps->cols+jj];
}
}
//cvRepeat(ss, m_sigmaPointst_1);
// Transform samples according to motion model
CvMat *ys;
CvMat *base;
CvMat *delta;
CvMat *reptemp;
int ysdim;
double *ysdb;
double *basedb;
double *deltadb;
double *reptempdb;
switch(model)
{
case MOTION_MODEL:
ysdim = dim-2;
ys = cvCreateMat(ysdim, N, CV_64FC1);
base = cvCreateMat(ysdim, N, CV_64FC1);
delta = cvCreateMat(ysdim, N, CV_64FC1);
reptemp = cvCreateMat(ysdim, 1, CV_64FC1);
cvRepeat(ss, ys);
ysdb = ys->data.db;
basedb = base->data.db;
deltadb = delta->data.db;
reptempdb = reptemp->data.db;
for(j=0; j<N; j++)
{
double v = ssdb[(dim-2)*N+j];
double g = ssdb[(dim-1)*N+j];
ysdb[0*N+j] = ssdb[0*N+j] + v*DT_CONTROLS*cos(g+ssdb[2*N+j]);
ysdb[1*N+j] = ssdb[1*N+j] + v*DT_CONTROLS*sin(g+ssdb[2*N+j]);
ysdb[2*N+j] = ssdb[2*N+j] + v*DT_CONTROLS*sin(g)/WHEELBASE;
}
for(i=0; i<ys->rows; i++) reptempdb[reptemp->cols*i] = ysdb[ys->cols*i];
cvRepeat(reptemp, base);
cvSub(base, ys, delta);
for(i=0; i<base->cols; i++) basedb[2*base->cols+i] = PI2PI(basedb[2*base->cols+i]);
cvSub(base, delta, ys);
//PrintCvMat(ys, "ys");
//PrintCvMat(reptemp, "reptemp");
//PrintCvMat(base, "base");
//PrintCvMat(delta, "delta");
//PrintCvMat(ys, "ys");
cvReleaseMat(&base);
cvReleaseMat(&delta);
cvReleaseMat(&reptemp);
//cvRepeat(ys, m_sigmaPointst);
break;
case AUGMENT_MODEL:
//PrintCvMat(ss, "ss");
ysdim = dim;
ys = cvCreateMat(dim, N, CV_64FC1);
double phi, r, b, s, c;
//PrintCvMat(XX, "XX");
for(j=0; j<N; j++)
{
phi = ssdb[2*ss->cols+j];
r = ssdb[(ss->rows-2)*ss->cols+j];
b = ssdb[(ss->rows-1)*ss->cols+j];
s = sin(phi+b);
c = cos(phi+b);
ssdb[(ss->rows-2)*ss->cols+j] = ssdb[0*ss->cols+j] + r*c;
ssdb[(ss->rows-1)*ss->cols+j] = ssdb[1*ss->cols+j] + r*s;
}
cvCopy(ss, ys);
//PrintCvMat(Ps, "Ps");
//PrintCvMat(ss, "ss");
break;
}
CvMat *y = cvCreateMat(ysdim, 1, CV_64FC1);
CvMat *dy = cvCreateMat(ysdim, N, CV_64FC1);
CvMat *dyu = cvCreateMat(ysdim, 1, CV_64FC1);
CvMat *dytu = cvCreateMat(1, ysdim, CV_64FC1);
CvMat *dyl = cvCreateMat(ysdim, N-1, CV_64FC1);
CvMat *dytl = cvCreateMat(N-1, ysdim, CV_64FC1);
CvMat *temp = cvCreateMat(ysdim, ysdim, CV_64FC1);
CvMat *Y = cvCreateMat(ysdim, ysdim, CV_64FC1);
CvMat *temp1 = cvCreateMat(ysdim, 1, CV_64FC1);
CvMat *repy = cvCreateMat(ysdim, N, CV_64FC1);
double *ydb = y->data.db;
double *dydb = dy->data.db;
double *dyudb = dyu->data.db;
double *dytudb = dytu->data.db;
double *dyldb = dyl->data.db;
double *dytldb = dytl->data.db;
double *tempdb = temp->data.db;
double *Ydb = Y->data.db;
double *temp1db = temp1->data.db;
double *repydb = repy->data.db;
ysdb = ys->data.db;
for(i=0; i<y->rows; i++) ydb[i] = 2*kappa*ysdb[ys->cols*i];
for(i=0; i<y->rows; i++)
{
for(j=1; j<N; j++)
{
ydb[i] += ysdb[ys->cols*i+j];
}
}
for(i=0; i<y->rows; i++) ydb[i] /= (2*scale);
for(i=0; i<y->rows; i++) temp1db[i] = ydb[y->cols*i];
cvRepeat(temp1, repy);
cvSub(ys, repy, dy);
// PrintCvMat(temp1, "temp1");
// PrintCvMat(repy, "repy");
// PrintCvMat(dy, "dy");
// for(j=0; j<N; j++)
// {
// dy->data.db[0*N+j] = ys->data.db[0*N+j] - y->data.db[0];
// dy->data.db[1*N+j] = ys->data.db[1*N+j] - y->data.db[1];
// dy->data.db[2*N+j] = ys->data.db[2*N+j] - y->data.db[2];
// }
for(i=0; i<ysdim; i++) dyudb[i] = dydb[i*dy->cols];
for(i=0; i<ysdim; i++) for(j=1, jj=0; j<N; j++, jj++)
dyldb[i*dyl->cols+jj] = dydb[i*dy->cols+j];
// PrintCvMat(ys, "ys");
// PrintCvMat(dy, "dy");
// PrintCvMat(dyu, "dyu");
// PrintCvMat(dyl, "dyl");
cvT(dyu, dytu);
cvT(dyl, dytl);
cvMatMul(dyu, dytu, Y);
// PrintCvMat(Y, "Y");
cvScale(Y, Y, 2*kappa);
// PrintCvMat(Y, "Y");
cvMatMul(dyl, dytl, temp);
// PrintCvMat(temp, "temp");
cvAdd(Y, temp, Y);
// PrintCvMat(Y, "Y");
cvScale(Y, Y, 1/(double)(2*scale));
// PrintCvMat(Y, "Y");
cvReleaseMat(&XX);
cvReleaseMat(&PX);
XX = cvCreateMat(y->rows, y->cols, CV_64FC1);
PX = cvCreateMat(Y->rows, Y->cols, CV_64FC1);
cvCopy(y, XX);
cvCopy(Y, PX);
// PrintCvMat(XX, "XX");
// PrintCvMat(PX, "PX");
cvReleaseMat(&D);
cvReleaseMat(&U);
cvReleaseMat(&V);
cvReleaseMat(&Ds);
cvReleaseMat(&UD);
cvReleaseMat(&Ps);
cvReleaseMat(&ss);
cvReleaseMat(&ys);
cvReleaseMat(&y);
cvReleaseMat(&dy);
cvReleaseMat(&dyu);
cvReleaseMat(&dytu);
cvReleaseMat(&dyl);
cvReleaseMat(&dytl);
cvReleaseMat(&temp);
cvReleaseMat(&Y);
cvReleaseMat(&temp1);
cvReleaseMat(&repy);
return 0;
}
bool CamAugmentation::CreateHomographyRotationTranslationMatrix( int c ){
int i;
// Get pointer to homography:
CvMat* m_homography = v_homography[c]->m_homography;
if( m_homography ){
// Vectors holding columns of H and R:
double a_H1[3];
CvMat m_H1 = cvMat( 3, 1, CV_64FC1, a_H1 );
for( i = 0; i < 3; i++ ) cvmSet( &m_H1, i, 0, cvmGet( m_homography, i, 0 ) );
double a_H2[3];
CvMat m_H2 = cvMat( 3, 1, CV_64FC1, a_H2 );
for( i = 0; i < 3; i++ ) cvmSet( &m_H2, i, 0, cvmGet( m_homography, i, 1 ) );
double a_H3[3];
CvMat m_H3 = cvMat( 3, 1, CV_64FC1, a_H3 );
for( i = 0; i < 3; i++ ) cvmSet( &m_H3, i, 0, cvmGet( m_homography, i, 2 ) );
double a_CinvH1[3];
CvMat m_CinvH1 = cvMat( 3, 1, CV_64FC1, a_CinvH1 );
double a_R1[3];
CvMat m_R1 = cvMat( 3, 1, CV_64FC1, a_R1 );
double a_R2[3];
CvMat m_R2 = cvMat( 3, 1, CV_64FC1, a_R2 );
double a_R3[3];
CvMat m_R3 = cvMat( 3, 1, CV_64FC1, a_R3 );
// The rotation matrix:
double a_R[9];
CvMat m_R = cvMat( 3, 3, CV_64FC1, a_R );
// The translation vector:
double a_T[3];
CvMat m_T = cvMat( 3, 1, CV_64FC1, a_T );
////////////////////////////////////////////////////////
// Create inverse calibration matrix:
CvMat* m_Cinv = InverseCalibrationMatrix( s_optimal.v_camera_c[c] );
// Create norming factor lambda:
cvGEMM( m_Cinv, &m_H1, 1, NULL, 0, &m_CinvH1, 0 );
// Search next orthonormal matrix:
if( cvNorm( &m_CinvH1, NULL, CV_L2, NULL ) != 0 ){
double lambda = 1.00/cvNorm( &m_CinvH1, NULL, CV_L2, NULL );
// Create normalized R1 & R2:
cvGEMM( m_Cinv, &m_H1, lambda, NULL, 0, &m_R1, 0 );
cvGEMM( m_Cinv, &m_H2, lambda, NULL, 0, &m_R2, 0 );
// Get R3 orthonormal to R1 and R2:
cvCrossProduct( &m_R1, &m_R2, &m_R3 );
// Put the rotation column vectors in the rotation matrix:
for( i = 0; i < 3; i++ ){
cvmSet( &m_R, i, 0, cvmGet( &m_R1, i, 0 ) );
cvmSet( &m_R, i, 1, cvmGet( &m_R2, i, 0 ) );
cvmSet( &m_R, i, 2, cvmGet( &m_R3, i, 0 ) );
}
// Calculate Translation Vector T (- because of its definition):
cvGEMM( m_Cinv, &m_H3, -lambda, NULL, 0, &m_T, 0 );
// Transformation of R into - in Frobenius sense - next orthonormal matrix:
double a_W[9]; CvMat m_W = cvMat( 3, 3, CV_64FC1, a_W );
double a_U[9]; CvMat m_U = cvMat( 3, 3, CV_64FC1, a_U );
double a_Vt[9]; CvMat m_Vt = cvMat( 3, 3, CV_64FC1, a_Vt );
cvSVD( &m_R, &m_W, &m_U, &m_Vt, CV_SVD_MODIFY_A | CV_SVD_V_T );
cvMatMul( &m_U, &m_Vt, &m_R );
// Put the rotation matrix and the translation vector together:
double a_view_to_cam[12];
CvMat m_view_to_cam = cvMat( 3, 4, CV_64FC1, a_view_to_cam );
CamCalibration::ComposeRotationTranslationTo3x4Matrix( &m_view_to_cam, &m_R, &m_T );
// Transfer to global reference coordinate system:
double a_cam_inv[12];
CvMat m_cam_inv = cvMat( 3, 4, CV_64FC1, a_cam_inv );
CamCalibration::Mat3x4Inverse( s_optimal.v_camera_r_t[c], &m_cam_inv );
CamCalibration::Mat3x4Mul( &m_cam_inv, &m_view_to_cam, v_homography_r_t );
return true;
}
}
return false;
}
void pca(IplImage *image)
{
int i, j, k;
int width, height, step, channels;
unsigned char *src;
float *u;
unsigned char *b;
unsigned char *g;
height = image->height;
width = image->width;
step = image->widthStep;
channels = image->nChannels;
src = (unsigned char *)image->imageData;
u = (float *)malloc(sizeof(float) * height * channels);
b = (unsigned char *)malloc(sizeof(unsigned char) * width * height * channels);
g = (unsigned char *)malloc(sizeof(unsigned char) * height * channels);
//bg = (unsigned char *)malloc(sizeof(unsigned char) * width * height);
//bb = (unsigned char *)malloc(sizeof(unsigned char) * width * height);
init_floatMat(u, height * channels);
init_charMat(b, height * width * channels);
//init_mat(bg, height * width);
//init_mat(bb, height * widht);
init_charMat(g, height * channels);
//begin by calculating the empirical mean
//u[1..height] = 1/n sum(src[i,j])
for(i = 0; i < height; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
//need to fix floating point arithmetic
u[i*channels + k] += (float)src[i*step+j*channels+k] / (float)width;
}
}
}
printf("empirical means working\n");
//we next calculate the deviation from the mean
//b = src[i,j] - u;
for(i = 0; i < height; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
b[i*step+j*channels+k] = src[i*step+j*channels+k] - (int)u[i*channels+k];
}
}
}
printf("deviation working\n");
//we now need to find the covariance matrix
//b in opencv matrix form
CvMat bMat = cvMat(height, width, CV_8UC3, b);
//CvMat bgMat = cvMat(height, width, CV_8U3, bg);
//CvMat bbMat = cvMat(height, width, CV_8U3, bb);
CvMat *bMatb = cvCreateMat(height, width, CV_8UC1);
CvMat *bMatg = cvCreateMat(height, width, CV_8UC1);
CvMat *bMatr = cvCreateMat(height, width, CV_8UC1);
cvSplit(&bMat, bMatb, bMatg, bMatr, 0);
//covariance matrix
CvMat *cb = cvCreateMat(height, height, CV_32FC1);
CvMat *cg = cvCreateMat(height, height, CV_32FC1);
CvMat *cr = cvCreateMat(height, height, CV_32FC1);
CvMat *c = cvCreateMat(height, height, CV_32FC3);
cvMulTransposed(bMatb, cb, 0, NULL, 1.0/(double)width);
cvMulTransposed(bMatg, cg, 0, NULL, 1.0/(double)width);
cvMulTransposed(bMatr, cr, 0, NULL, 1.0/(double)width);
printf("Covariance Matrix working\n");
//eigenvector and values
CvMat *eMatb = cvCreateMat(height, height, CV_32FC1);
CvMat *lMatb = cvCreateMat(height, 1, CV_32FC1);
CvMat *eMatr = cvCreateMat(height, height, CV_32FC1);
CvMat *lMatr = cvCreateMat(height, 1, CV_32FC1);
CvMat *eMatg = cvCreateMat(height, height, CV_32FC1);
CvMat *lMatg = cvCreateMat(height, 1, CV_32FC1);
printf("Successfully created eigen matrices\n");
//cvEigenVV(cb, eMatb, lMatb, 1e-10, -1, -1);
//cvEigenVV(cg, eMatg, lMatg, 1e-10, -1, -1);
//cvEigenVV(cr, eMatr, lMatr, 1e-10, -1, -1);
cvSVD(cb, lMatb, eMatb, NULL, CV_SVD_U_T & CV_SVD_MODIFY_A);
cvSVD(cg, lMatg, eMatg, NULL, CV_SVD_U_T & CV_SVD_MODIFY_A);
cvSVD(cr, lMatr, eMatr, NULL, CV_SVD_U_T & CV_SVD_MODIFY_A);
printf("Eigentvectors and Eigenvalues passes\n");
unsigned char *lb = lMatb->data.ptr;
unsigned char *eb = eMatb->data.ptr;
unsigned char *lr = lMatr->data.ptr;
unsigned char *er = eMatr->data.ptr;
unsigned char *lg = lMatg->data.ptr;
unsigned char *eg = eMatg->data.ptr;
for(i = 0; i < height; i++)
{
for(j = 0; j < i+1; j++)
{
for(k = 0; k < channels; k++)
{
g[i*channels] += lb[i*channels+k];
}
}
}
printf("Successfully computed cumulative energy\n");
int L = 0;
float currVal = 0.0;
/*for(i = 0; i < height; i++)
{
if(currVal >= 0.9)
{
L = i;
break;
}
currVal = 0.0;
for(k = 0; k < channels; k++)
{
currVal += (float)g[i*channels+k] / (float)g[height - 3 + k];
}
}*/
L = 2;
printf("Successfully computed L with value of %d\n", L);
unsigned char *w;
w = (unsigned char *)malloc(sizeof(unsigned char) * height * L * channels);
for(i = 0; i < height; i++)
{
for(j = 0; j < L; j++)
{
//for(k = 0; k < channels; k++)
//{
w[i*(L*channels)+j*channels] = eb[i*height+j];
w[i*(L*channels)+j*channels+1] = eg[i*height+j];
w[i*(L*channels)+j*channels+2] = er[i*height+j];
//}
}
}
printf("Successfully created basis vectors\n");
unsigned char *s;
s = (unsigned char *)malloc(sizeof(unsigned char) * height * channels);
for(i = 0; i < height; i++)
{
s[i*channels] = sqrt(cb->data.ptr[i*cb->cols+i]);
s[i*channels+1] = sqrt(cg->data.ptr[i*cg->cols+i]);
s[i*channels+2] = sqrt(cr->data.ptr[i*cr->cols+i]);
}
printf("Successfully convertd source data to z-scores\n");
unsigned char *z;
z = (unsigned char *)malloc(sizeof(unsigned char) * height * width * channels);
for(i = 0; i < height; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
z[i*step+j*channels+k] = (float)b[i*step+j*channels+k] / (float)s[i*channels+k];
}
}
}
printf("Successfully calculated Z\n");
//Projecting the z-scores of the data onto the new basis
//CvMat wMatb = cvMat(height, L, CV_32FC1, eb);
//CvMat wMatr = cvMat(height, L, CV_32FC1, er);
//CvMat wMatg = cvMat(height, L, CV_32FC1, eg);
CvMat wMat = cvMat(L, height, CV_32FC3, w);
//cvMerge(&wMatb, &wMatr, &wMatg, 0, wMat);
CvMat *wMatT = cvCreateMat(height, L, CV_32FC3);
cvTranspose(&wMat, wMatT);
//char *dat = wMatT->data.ptr;
/*for(i = 0; i < L; i++)
{
for(j = 0; j < height; j++)
{
for(k = 0; k < channels; k++)
{
printf("%d ", dat[i*L+j*channels+k]);
}
printf("\n");
}
}*/
//CvMat *wMatTb = cvCreateMat(height, height, CV_8UC1);
//CvMat *wMatTg = cvCreateMat(height, height, CV_8UC1);
//CvMat *wMatTr = cvCreateMat(height, height, CV_8UC1);
//cvSplit(wMatT, wMatTb, wMatTg, wMatTr, 0);
printf("Transpose of W\n");
CvMat zMat = cvMat(height, width, CV_32FC3, z);
//CvMat *zMatb = cvCreateMat(height, width, CV_8UC1);
//CvMat *zMatg = cvCreateMat(height, width, CV_8UC1);
//CvMat *zMatr = cvCreateMat(height, width, CV_8UC1);
//cvSplit(&zMat, zMatb, zMatg, zMatr, 0);
printf("created z matrix\n");
CvMat *yMat = cvCreateMat(height, width, CV_32FC3);
//CvMat *yMatb = cvCreateMat(height, width, CV_8UC1);
//CvMat *yMatg = cvCreateMat(height, width, CV_8UC1);
//CvMat *yMatr = cvCreateMat(height, width, CV_8UC1);
printf("computed y matrix\n");
char *wdat = wMatT->data.ptr;
char *zdat = zMat.data.ptr;
char *ydat = (char *)malloc(sizeof(char) * L * width * channels);
init_charMat(ydat, L*width*channels);
int r = 0;
for(i = 0; i < L; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
for(r = 0; r < width; r++)
{
ydat[i*step+j*channels+k] += wdat[i*step+r*channels+k] * zdat[r*step+j*channels+k];
}
}
}
}
//char *fnorm = (char *)malloc(sizeof(char) * width);
/*for(i = 0; i < width * channels; i++)
{
printf("%d\n", ydat[i]);
}*/
/*float adotb = cvDotProduct(wMatT, &zMat);
float bdota = cvDotProduct(&zMat, wMatT);
float div = adotb/bdota;
cvDiv(NULL, &zMat, yMat, div);*/
//cvMul(wMatT, &zMat, yMat, 1.0);
//cvMul(wMatTg, zMatg, yMatg, 1.0);
//cvMul(wMatTr, zMatr, yMatr, 1.0);
printf("Matrix Multiply Successful\n");
//char *output = yMat->data.ptr;
//printf("height: %d width: %d channels: %d", height, width, channels);
for(i = 0; i < height; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
src[i*step+j*channels+k] = src[i*step+j*channels+k] * ydat[(i*channels)+width+k];
}
}
}
printf("Successfully multiplied\n");
//cvMerge(yMatb, yMatg, yMatr, 0, yMat);
}
void CvEM::init_em( const CvVectors& train_data )
{
CvMat *w = 0, *u = 0, *tcov = 0;
CV_FUNCNAME( "CvEM::init_em" );
__BEGIN__;
double maxval = 0;
int i, force_symm_plus = 0;
int nclusters = params.nclusters, nsamples = train_data.count, dims = train_data.dims;
if( params.start_step == START_AUTO_STEP || nclusters == 1 || nclusters == nsamples )
init_auto( train_data );
else if( params.start_step == START_M_STEP )
{
for( i = 0; i < nsamples; i++ )
{
CvMat prob;
cvGetRow( params.probs, &prob, i );
cvMaxS( &prob, 0., &prob );
cvMinMaxLoc( &prob, 0, &maxval );
if( maxval < FLT_EPSILON )
cvSet( &prob, cvScalar(1./nclusters) );
else
cvNormalize( &prob, &prob, 1., 0, CV_L1 );
}
EXIT; // do not preprocess covariation matrices,
// as in this case they are initialized at the first iteration of EM
}
else
{
CV_ASSERT( params.start_step == START_E_STEP && params.means );
if( params.weights && params.covs )
{
cvConvert( params.means, means );
cvReshape( weights, weights, 1, params.weights->rows );
cvConvert( params.weights, weights );
cvReshape( weights, weights, 1, 1 );
cvMaxS( weights, 0., weights );
cvMinMaxLoc( weights, 0, &maxval );
if( maxval < FLT_EPSILON )
cvSet( weights, cvScalar(1./nclusters) );
cvNormalize( weights, weights, 1., 0, CV_L1 );
for( i = 0; i < nclusters; i++ )
CV_CALL( cvConvert( params.covs[i], covs[i] ));
force_symm_plus = 1;
}
else
init_auto( train_data );
}
CV_CALL( tcov = cvCreateMat( dims, dims, CV_64FC1 ));
CV_CALL( w = cvCreateMat( dims, dims, CV_64FC1 ));
if( params.cov_mat_type == COV_MAT_GENERIC )
CV_CALL( u = cvCreateMat( dims, dims, CV_64FC1 ));
for( i = 0; i < nclusters; i++ )
{
if( force_symm_plus )
{
cvTranspose( covs[i], tcov );
cvAddWeighted( covs[i], 0.5, tcov, 0.5, 0, tcov );
}
else
cvCopy( covs[i], tcov );
cvSVD( tcov, w, u, 0, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
if( params.cov_mat_type == COV_MAT_SPHERICAL )
cvSetIdentity( covs[i], cvScalar(cvTrace(w).val[0]/dims) );
else if( params.cov_mat_type == COV_MAT_DIAGONAL )
cvCopy( w, covs[i] );
else
{
// generic case: covs[i] = (u')'*max(w,0)*u'
cvGEMM( u, w, 1, 0, 0, tcov, CV_GEMM_A_T );
cvGEMM( tcov, u, 1, 0, 0, covs[i], 0 );
}
}
__END__;
cvReleaseMat( &w );
cvReleaseMat( &u );
cvReleaseMat( &tcov );
}
bool
cvFindExtrinsicCameraParams3( const CvMat* obj_points,
const CvMat* img_points, const CvMat* A,
const CvMat* dist_coeffs,
CvMat* r_vec, CvMat* t_vec )
{
bool fGood = true;
const int max_iter = 20;
CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0;
CV_FUNCNAME( "cvFindExtrinsicCameraParams3" );
__BEGIN__;
int i, j, count;
double a[9], k[4] = { 0, 0, 0, 0 }, R[9], ifx, ify, cx, cy;
double Mc[3] = {0, 0, 0}, MM[9], U[9], V[9], W[3];
double JtJ[6*6], JtErr[6], JtJW[6], JtJV[6*6], delta[6], param[6];
CvPoint3D64f* M = 0;
CvPoint2D64f *m = 0, *mn = 0;
CvMat _a = cvMat( 3, 3, CV_64F, a );
CvMat _R = cvMat( 3, 3, CV_64F, R );
CvMat _r = cvMat( 3, 1, CV_64F, param );
CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
CvMat _Mc = cvMat( 1, 3, CV_64F, Mc );
CvMat _MM = cvMat( 3, 3, CV_64F, MM );
CvMat _U = cvMat( 3, 3, CV_64F, U );
CvMat _V = cvMat( 3, 3, CV_64F, V );
CvMat _W = cvMat( 3, 1, CV_64F, W );
CvMat _JtJ = cvMat( 6, 6, CV_64F, JtJ );
CvMat _JtErr = cvMat( 6, 1, CV_64F, JtErr );
CvMat _JtJW = cvMat( 6, 1, CV_64F, JtJW );
CvMat _JtJV = cvMat( 6, 6, CV_64F, JtJV );
CvMat _delta = cvMat( 6, 1, CV_64F, delta );
CvMat _param = cvMat( 6, 1, CV_64F, param );
CvMat _dpdr, _dpdt;
if( !CV_IS_MAT(obj_points) || !CV_IS_MAT(img_points) ||
!CV_IS_MAT(A) || !CV_IS_MAT(r_vec) || !CV_IS_MAT(t_vec) )
CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" );
count = MAX(obj_points->cols, obj_points->rows);
CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 ));
M = (CvPoint3D64f*)_M->data.db;
m = (CvPoint2D64f*)_m->data.db;
mn = (CvPoint2D64f*)_mn->data.db;
CV_CALL( cvConvertPointsHomogenious( obj_points, _M ));
CV_CALL( cvConvertPointsHomogenious( img_points, _m ));
CV_CALL( cvConvert( A, &_a ));
if( dist_coeffs )
{
CvMat _k;
if( !CV_IS_MAT(dist_coeffs) ||
CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
CV_MAT_DEPTH(dist_coeffs->type) != CV_32F ||
dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 )
CV_ERROR( CV_StsBadArg,
"Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
_k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(dist_coeffs->type)), k );
CV_CALL( cvConvert( dist_coeffs, &_k ));
}
if( CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F ||
r_vec->rows != 1 && r_vec->cols != 1 ||
r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3 )
CV_ERROR( CV_StsBadArg, "Rotation vector must be 1x3 or 3x1 floating-point vector" );
if( CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F ||
t_vec->rows != 1 && t_vec->cols != 1 ||
t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
CV_ERROR( CV_StsBadArg,
"Translation vector must be 1x3 or 3x1 floating-point vector" );
ifx = 1./a[0]; ify = 1./a[4];
cx = a[2]; cy = a[5];
// normalize image points
// (unapply the intrinsic matrix transformation and distortion)
for( i = 0; i < count; i++ )
{
double x = (m[i].x - cx)*ifx, y = (m[i].y - cy)*ify, x0 = x, y0 = y;
// compensate distortion iteratively
if( dist_coeffs )
for( j = 0; j < 5; j++ )
{
double r2 = x*x + y*y;
double icdist = 1./(1 + k[0]*r2 + k[1]*r2*r2);
double delta_x = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
double delta_y = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
x = (x0 - delta_x)*icdist;
y = (y0 - delta_y)*icdist;
}
mn[i].x = x; mn[i].y = y;
// calc mean(M)
Mc[0] += M[i].x;
Mc[1] += M[i].y;
Mc[2] += M[i].z;
}
Mc[0] /= count;
Mc[1] /= count;
Mc[2] /= count;
cvReshape( _M, _M, 1, count );
cvMulTransposed( _M, &_MM, 1, &_Mc );
cvSVD( &_MM, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
// initialize extrinsic parameters
if( W[2]/W[1] < 1e-3 || count < 4 )
{
// a planar structure case (all M's lie in the same plane)
double tt[3], h[9], h1_norm, h2_norm;
CvMat* R_transform = &_V;
CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
CvMat _H = cvMat( 3, 3, CV_64F, h );
CvMat _h1, _h2, _h3;
if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
cvSetIdentity( R_transform );
if( cvDet(R_transform) < 0 )
cvScale( R_transform, R_transform, -1 );
cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
for( i = 0; i < count; i++ )
{
const double* Rp = R_transform->data.db;
const double* Tp = T_transform.data.db;
const double* src = _M->data.db + i*3;
double* dst = _Mxy->data.db + i*2;
dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
}
cvFindHomography( _Mxy, _mn, &_H );
cvGetCol( &_H, &_h1, 0 );
_h2 = _h1; _h2.data.db++;
_h3 = _h2; _h3.data.db++;
h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
cvScale( &_h1, &_h1, 1./h1_norm );
cvScale( &_h2, &_h2, 1./h2_norm );
cvScale( &_h3, &_t, 2./(h1_norm + h2_norm));
cvCrossProduct( &_h1, &_h2, &_h3 );
cvRodrigues2( &_H, &_r );
cvRodrigues2( &_r, &_H );
cvMatMulAdd( &_H, &T_transform, &_t, &_t );
cvMatMul( &_H, R_transform, &_R );
cvRodrigues2( &_R, &_r );
}
else
{
// non-planar structure. Use DLT method
double* L;
double LL[12*12], LW[12], LV[12*12], sc;
CvMat _LL = cvMat( 12, 12, CV_64F, LL );
CvMat _LW = cvMat( 12, 1, CV_64F, LW );
CvMat _LV = cvMat( 12, 12, CV_64F, LV );
CvMat _RRt, _RR, _tt;
CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F ));
L = _L->data.db;
for( i = 0; i < count; i++, L += 24 )
{
double x = -mn[i].x, y = -mn[i].y;
L[0] = L[16] = M[i].x;
L[1] = L[17] = M[i].y;
L[2] = L[18] = M[i].z;
L[3] = L[19] = 1.;
L[4] = L[5] = L[6] = L[7] = 0.;
L[12] = L[13] = L[14] = L[15] = 0.;
L[8] = x*M[i].x;
L[9] = x*M[i].y;
L[10] = x*M[i].z;
L[11] = x;
L[20] = y*M[i].x;
L[21] = y*M[i].y;
L[22] = y*M[i].z;
L[23] = y;
}
cvMulTransposed( _L, &_LL, 1 );
cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
_RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
cvGetCols( &_RRt, &_RR, 0, 3 );
cvGetCol( &_RRt, &_tt, 3 );
if( cvDet(&_RR) < 0 )
cvScale( &_RRt, &_RRt, -1 );
sc = cvNorm(&_RR);
cvSVD( &_RR, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
cvScale( &_tt, &_t, cvNorm(&_R)/sc );
cvRodrigues2( &_R, &_r );
cvReleaseMat( &_L );
}
//
// Check if new r and t are good
//
if ( cvGetReal1D( r_vec, 0 ) ||
cvGetReal1D( r_vec, 1 ) ||
cvGetReal1D( r_vec, 2 ) ||
cvGetReal1D( t_vec, 0 ) ||
cvGetReal1D( t_vec, 1 ) ||
cvGetReal1D( t_vec, 2 ) )
{
//
// perfom this only when the old r and t exist.
//
CvMat * R_inv = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * r_old = cvCreateMat( 3, 1, CV_64FC1 );
CvMat * R_old = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * t_old = cvCreateMat( 3, 1, CV_64FC1 );
// get new center
cvInvert( &_R, R_inv );
double new_center[3];
CvMat newCenter = cvMat( 3, 1, CV_64FC1, new_center );
cvMatMul( R_inv, &_t, &newCenter );
cvScale( &newCenter, &newCenter, -1 );
// get old center
cvConvert( r_vec, r_old );
cvConvert( t_vec, t_old );
cvRodrigues2( r_old, R_old );
cvInvert( R_old, R_inv );
double old_center[3];
CvMat oldCenter = cvMat( 3, 1, CV_64FC1, old_center );
cvMatMul( R_inv, t_old, &oldCenter );
cvScale( &oldCenter, &oldCenter, -1 );
// get difference
double diff_center = 0;
for ( int i = 0; i < 3 ; i ++ )
diff_center += pow( new_center[i] - old_center[i], 2 );
diff_center = sqrt( diff_center );
if ( diff_center > 300 )
{
printf("diff_center = %.2f --> set initial r and t as same as the previous\n", diff_center);
cvConvert(r_vec, &_r);
cvConvert(t_vec, &_t);
fGood = false;
}
// else printf("diff_center = %.2f\n", diff_center );
cvReleaseMat( &R_inv );
cvReleaseMat( &r_old );
cvReleaseMat( &R_old );
cvReleaseMat( &t_old );
}
if ( fGood )
{
CV_CALL( _J = cvCreateMat( 2*count, 6, CV_64FC1 ));
cvGetCols( _J, &_dpdr, 0, 3 );
cvGetCols( _J, &_dpdt, 3, 6 );
// refine extrinsic parameters using iterative algorithm
for( i = 0; i < max_iter; i++ )
{
double n1, n2;
cvReshape( _mn, _mn, 2, 1 );
cvProjectPoints2( _M, &_r, &_t, &_a, dist_coeffs,
_mn, &_dpdr, &_dpdt, 0, 0, 0 );
cvSub( _m, _mn, _mn );
cvReshape( _mn, _mn, 1, 2*count );
cvMulTransposed( _J, &_JtJ, 1 );
cvGEMM( _J, _mn, 1, 0, 0, &_JtErr, CV_GEMM_A_T );
cvSVD( &_JtJ, &_JtJW, 0, &_JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
if( JtJW[5]/JtJW[0] < 1e-12 )
break;
cvSVBkSb( &_JtJW, &_JtJV, &_JtJV, &_JtErr,
&_delta, CV_SVD_U_T + CV_SVD_V_T );
cvAdd( &_delta, &_param, &_param );
n1 = cvNorm( &_delta );
n2 = cvNorm( &_param );
if( n1/n2 < 1e-10 )
break;
}
_r = cvMat( r_vec->rows, r_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), param );
_t = cvMat( t_vec->rows, t_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), param + 3 );
cvConvert( &_r, r_vec );
cvConvert( &_t, t_vec );
}
__END__;
cvReleaseMat( &_M );
cvReleaseMat( &_Mxy );
cvReleaseMat( &_m );
cvReleaseMat( &_mn );
cvReleaseMat( &_L );
cvReleaseMat( &_J );
return fGood;
}
/* log_weight_div_det[k] = -2*log(weights_k) + log(det(Sigma_k)))
covs[k] = cov_rotate_mats[k] * cov_eigen_values[k] * (cov_rotate_mats[k])'
cov_rotate_mats[k] are orthogonal matrices of eigenvectors and
cov_eigen_values[k] are diagonal matrices (represented by 1D vectors) of eigen values.
The <alpha_ik> is the probability of the vector x_i to belong to the k-th cluster:
<alpha_ik> ~ weights_k * exp{ -0.5[ln(det(Sigma_k)) + (x_i - mu_k)' Sigma_k^(-1) (x_i - mu_k)] }
We calculate these probabilities here by the equivalent formulae:
Denote
S_ik = -0.5(log(det(Sigma_k)) + (x_i - mu_k)' Sigma_k^(-1) (x_i - mu_k)) + log(weights_k),
M_i = max_k S_ik = S_qi, so that the q-th class is the one where maximum reaches. Then
alpha_ik = exp{ S_ik - M_i } / ( 1 + sum_j!=q exp{ S_ji - M_i })
*/
double CvEM::run_em( const CvVectors& train_data )
{
CvMat* centered_sample = 0;
CvMat* covs_item = 0;
CvMat* log_det = 0;
CvMat* log_weights = 0;
CvMat* cov_eigen_values = 0;
CvMat* samples = 0;
CvMat* sum_probs = 0;
log_likelihood = -DBL_MAX;
CV_FUNCNAME( "CvEM::run_em" );
__BEGIN__;
int nsamples = train_data.count, dims = train_data.dims, nclusters = params.nclusters;
double min_variation = FLT_EPSILON;
double min_det_value = MAX( DBL_MIN, pow( min_variation, dims ));
double likelihood_bias = -CV_LOG2PI * (double)nsamples * (double)dims / 2., _log_likelihood = -DBL_MAX;
int start_step = params.start_step;
int i, j, k, n;
int is_general = 0, is_diagonal = 0, is_spherical = 0;
double prev_log_likelihood = -DBL_MAX / 1000., det, d;
CvMat whdr, iwhdr, diag, *w, *iw;
double* w_data;
double* sp_data;
if( nclusters == 1 )
{
double log_weight;
CV_CALL( cvSet( probs, cvScalar(1.)) );
if( params.cov_mat_type == COV_MAT_SPHERICAL )
{
d = cvTrace(*covs).val[0]/dims;
d = MAX( d, FLT_EPSILON );
inv_eigen_values->data.db[0] = 1./d;
log_weight = pow( d, dims*0.5 );
}
else
{
w_data = inv_eigen_values->data.db;
if( params.cov_mat_type == COV_MAT_GENERIC )
cvSVD( *covs, inv_eigen_values, *cov_rotate_mats, 0, CV_SVD_U_T );
else
cvTranspose( cvGetDiag(*covs, &diag), inv_eigen_values );
cvMaxS( inv_eigen_values, FLT_EPSILON, inv_eigen_values );
for( j = 0, det = 1.; j < dims; j++ )
det *= w_data[j];
log_weight = sqrt(det);
cvDiv( 0, inv_eigen_values, inv_eigen_values );
}
log_weight_div_det->data.db[0] = -2*log(weights->data.db[0]/log_weight);
log_likelihood = DBL_MAX/1000.;
EXIT;
}
if( params.cov_mat_type == COV_MAT_GENERIC )
is_general = 1;
else if( params.cov_mat_type == COV_MAT_DIAGONAL )
is_diagonal = 1;
else if( params.cov_mat_type == COV_MAT_SPHERICAL )
is_spherical = 1;
/* In the case of <cov_mat_type> == COV_MAT_DIAGONAL, the k-th row of cov_eigen_values
contains the diagonal elements (variations). In the case of
<cov_mat_type> == COV_MAT_SPHERICAL - the 0-ths elements of the vectors cov_eigen_values[k]
are to be equal to the mean of the variations over all the dimensions. */
CV_CALL( log_det = cvCreateMat( 1, nclusters, CV_64FC1 ));
CV_CALL( log_weights = cvCreateMat( 1, nclusters, CV_64FC1 ));
CV_CALL( covs_item = cvCreateMat( dims, dims, CV_64FC1 ));
CV_CALL( centered_sample = cvCreateMat( 1, dims, CV_64FC1 ));
CV_CALL( cov_eigen_values = cvCreateMat( inv_eigen_values->rows, inv_eigen_values->cols, CV_64FC1 ));
CV_CALL( samples = cvCreateMat( nsamples, dims, CV_64FC1 ));
CV_CALL( sum_probs = cvCreateMat( 1, nclusters, CV_64FC1 ));
sp_data = sum_probs->data.db;
// copy the training data into double-precision matrix
for( i = 0; i < nsamples; i++ )
{
const float* src = train_data.data.fl[i];
double* dst = (double*)(samples->data.ptr + samples->step*i);
for( j = 0; j < dims; j++ )
dst[j] = src[j];
}
if( start_step != START_M_STEP )
{
for( k = 0; k < nclusters; k++ )
{
if( is_general || is_diagonal )
{
w = cvGetRow( cov_eigen_values, &whdr, k );
if( is_general )
cvSVD( covs[k], w, cov_rotate_mats[k], 0, CV_SVD_U_T );
else
cvTranspose( cvGetDiag( covs[k], &diag ), w );
w_data = w->data.db;
for( j = 0, det = 1.; j < dims; j++ )
det *= w_data[j];
if( det < min_det_value )
{
if( start_step == START_AUTO_STEP )
det = min_det_value;
else
EXIT;
}
log_det->data.db[k] = det;
}
else
{
d = cvTrace(covs[k]).val[0]/(double)dims;
if( d < min_variation )
{
if( start_step == START_AUTO_STEP )
d = min_variation;
else
EXIT;
}
cov_eigen_values->data.db[k] = d;
log_det->data.db[k] = d;
}
}
cvLog( log_det, log_det );
if( is_spherical )
cvScale( log_det, log_det, dims );
}
for( n = 0; n < params.term_crit.max_iter; n++ )
{
if( n > 0 || start_step != START_M_STEP )
{
// e-step: compute probs_ik from means_k, covs_k and weights_k.
CV_CALL(cvLog( weights, log_weights ));
// S_ik = -0.5[log(det(Sigma_k)) + (x_i - mu_k)' Sigma_k^(-1) (x_i - mu_k)] + log(weights_k)
for( k = 0; k < nclusters; k++ )
{
CvMat* u = cov_rotate_mats[k];
const double* mean = (double*)(means->data.ptr + means->step*k);
w = cvGetRow( cov_eigen_values, &whdr, k );
iw = cvGetRow( inv_eigen_values, &iwhdr, k );
cvDiv( 0, w, iw );
w_data = (double*)(inv_eigen_values->data.ptr + inv_eigen_values->step*k);
for( i = 0; i < nsamples; i++ )
{
double *csample = centered_sample->data.db, p = log_det->data.db[k];
const double* sample = (double*)(samples->data.ptr + samples->step*i);
double* pp = (double*)(probs->data.ptr + probs->step*i);
for( j = 0; j < dims; j++ )
csample[j] = sample[j] - mean[j];
if( is_general )
cvGEMM( centered_sample, u, 1, 0, 0, centered_sample, CV_GEMM_B_T );
for( j = 0; j < dims; j++ )
p += csample[j]*csample[j]*w_data[is_spherical ? 0 : j];
pp[k] = -0.5*p + log_weights->data.db[k];
// S_ik <- S_ik - max_j S_ij
if( k == nclusters - 1 )
{
double max_val = 0;
for( j = 0; j < nclusters; j++ )
max_val = MAX( max_val, pp[j] );
for( j = 0; j < nclusters; j++ )
pp[j] -= max_val;
}
}
}
CV_CALL(cvExp( probs, probs )); // exp( S_ik )
cvZero( sum_probs );
// alpha_ik = exp( S_ik ) / sum_j exp( S_ij ),
// log_likelihood = sum_i log (sum_j exp(S_ij))
for( i = 0, _log_likelihood = likelihood_bias; i < nsamples; i++ )
{
double* pp = (double*)(probs->data.ptr + probs->step*i), sum = 0;
for( j = 0; j < nclusters; j++ )
sum += pp[j];
sum = 1./MAX( sum, DBL_EPSILON );
for( j = 0; j < nclusters; j++ )
{
double p = pp[j] *= sum;
sp_data[j] += p;
}
_log_likelihood -= log( sum );
}
// check termination criteria
if( fabs( (_log_likelihood - prev_log_likelihood) / prev_log_likelihood ) < params.term_crit.epsilon )
break;
prev_log_likelihood = _log_likelihood;
}
// m-step: update means_k, covs_k and weights_k from probs_ik
cvGEMM( probs, samples, 1, 0, 0, means, CV_GEMM_A_T );
for( k = 0; k < nclusters; k++ )
{
double sum = sp_data[k], inv_sum = 1./sum;
CvMat* cov = covs[k], _mean, _sample;
w = cvGetRow( cov_eigen_values, &whdr, k );
w_data = w->data.db;
cvGetRow( means, &_mean, k );
cvGetRow( samples, &_sample, k );
// update weights_k
weights->data.db[k] = sum;
// update means_k
cvScale( &_mean, &_mean, inv_sum );
// compute covs_k
cvZero( cov );
cvZero( w );
for( i = 0; i < nsamples; i++ )
{
double p = probs->data.db[i*nclusters + k]*inv_sum;
_sample.data.db = (double*)(samples->data.ptr + samples->step*i);
if( is_general )
{
cvMulTransposed( &_sample, covs_item, 1, &_mean );
cvScaleAdd( covs_item, cvRealScalar(p), cov, cov );
}
else
for( j = 0; j < dims; j++ )
{
double val = _sample.data.db[j] - _mean.data.db[j];
w_data[is_spherical ? 0 : j] += p*val*val;
}
}
if( is_spherical )
{
d = w_data[0]/(double)dims;
d = MAX( d, min_variation );
w->data.db[0] = d;
log_det->data.db[k] = d;
}
else
{
if( is_general )
cvSVD( cov, w, cov_rotate_mats[k], 0, CV_SVD_U_T );
cvMaxS( w, min_variation, w );
for( j = 0, det = 1.; j < dims; j++ )
det *= w_data[j];
log_det->data.db[k] = det;
}
}
cvConvertScale( weights, weights, 1./(double)nsamples, 0 );
cvMaxS( weights, DBL_MIN, weights );
cvLog( log_det, log_det );
if( is_spherical )
cvScale( log_det, log_det, dims );
} // end of iteration process
//log_weight_div_det[k] = -2*log(weights_k/det(Sigma_k))^0.5) = -2*log(weights_k) + log(det(Sigma_k)))
if( log_weight_div_det )
{
cvScale( log_weights, log_weight_div_det, -2 );
cvAdd( log_weight_div_det, log_det, log_weight_div_det );
}
/* Now finalize all the covariation matrices:
1) if <cov_mat_type> == COV_MAT_DIAGONAL we used array of <w> as diagonals.
Now w[k] should be copied back to the diagonals of covs[k];
2) if <cov_mat_type> == COV_MAT_SPHERICAL we used the 0-th element of w[k]
as an average variation in each cluster. The value of the 0-th element of w[k]
should be copied to the all of the diagonal elements of covs[k]. */
if( is_spherical )
{
for( k = 0; k < nclusters; k++ )
cvSetIdentity( covs[k], cvScalar(cov_eigen_values->data.db[k]));
}
else if( is_diagonal )
{
for( k = 0; k < nclusters; k++ )
cvTranspose( cvGetRow( cov_eigen_values, &whdr, k ),
cvGetDiag( covs[k], &diag ));
}
cvDiv( 0, cov_eigen_values, inv_eigen_values );
log_likelihood = _log_likelihood;
__END__;
cvReleaseMat( &log_det );
cvReleaseMat( &log_weights );
cvReleaseMat( &covs_item );
cvReleaseMat( ¢ered_sample );
cvReleaseMat( &cov_eigen_values );
cvReleaseMat( &samples );
cvReleaseMat( &sum_probs );
return log_likelihood;
}
bool SparseRec2View::estimateRelativePose()
{
double U[9], S[9], VT[9];
double W[9] =
{ 0.0, -1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, 1.0 };
double WT[9] =
{ 0.0, 1.0, 0.0,
-1.0, 0.0, 0.0,
0.0, 0.0, 1.0 };
double E[9];
double tmp9[9];
double u3[3], Ra[9], Rb[9];
CreateCvMatHead(_U,3,3,U);
CreateCvMatHead(_S,3,3,S);
CreateCvMatHead(_VT,3,3,VT);
CreateCvMatHead(_K,3,3,K);
CreateCvMatHead(_E,3,3,E);
CreateCvMatHead(_F,3,3,F);
CreateCvMatHead(_tmp9,3,3,tmp9);
CreateCvMatHead(_u3,3,1,u3);
CreateCvMatHead(_Ra,3,3,Ra);
CreateCvMatHead(_Rb,3,3,Rb);
CreateCvMatHead(_W,3,3,W);
CreateCvMatHead(_WT,3,3,WT);
//get essential matrix
cvGEMM(&_K,&_F,1,0,0,&_tmp9, CV_GEMM_A_T);
cvMatMul(&_tmp9, &_K, &_E); //E = K2' * F * K1; % K2==K1 currently
cvSVD(&_E, &_S, &_U, &_VT, CV_SVD_V_T);
/* Now find R and t */
u3[0] = U[2]; u3[1] = U[5]; u3[2] = U[8]; //u3 = U*[0;0;1];
//two possible R UDVT, UDTVT
cvMatMul(&_U, &_W, &_tmp9);
cvMatMul(&_tmp9, &_VT, &_Ra); //Ra = U*W*V';
cvMatMul(&_U, &_WT, &_tmp9);
cvMatMul(&_tmp9, &_VT, &_Rb); //Rb = U*W'*V';
if( cvDet(&_Ra) <0 )
cvScale(&_Ra, &_Ra, -1);
if( cvDet(&_Rb) <0 )
cvScale(&_Rb, &_Rb, -1);
double P1[12] = {
K[0],K[1],K[2],0,
K[3],K[4],K[5],0,
K[6],K[7],K[8],0
};
double P2[12];
CreateCvMatHead(_P2,3,4,P2);
// test Ra
P2[0]=Ra[0]; P2[1]=Ra[1]; P2[2]=Ra[2]; P2[3]=u3[0];
P2[4]=Ra[3]; P2[5]=Ra[4]; P2[6]=Ra[5]; P2[7]=u3[1];
P2[8]=Ra[6]; P2[9]=Ra[7]; P2[10]=Ra[8]; P2[11]=u3[2];
cvMatMul(&_K,&_P2,&_P2); //P2 = K*[Ra,u3];
int c1_pos = 0, c1_neg = 0;
int c2_pos = 0, c2_neg = 0;
for(int i=0; i<(int)p1.size(); ++i) {
if(!inliers[i]) continue;
double X[3];
helper::triangulate(p1[i].x, p1[i].y,
p2[i].x, p2[i].y, P1, P2, X);
double X2[3]={X[0],X[1],X[2]};
CreateCvMatHead(_X2,3,1,X2);
cvMatMul(&_Ra, &_X2, &_X2);
cvAdd(&_X2, &_u3, &_X2);
if (X[2] > 0) c1_pos++;
else c1_neg++;
if (X2[2] > 0) c2_pos++;
else c2_neg++;
}
//cout<<"Test Ra"<<endl;
//cout<<"+c1="<<c1_pos<<"\t-c1="<<c1_neg<<endl;
//cout<<"+c2="<<c2_pos<<"\t-c2="<<c2_neg<<endl;
if (c1_pos > c1_neg && c2_pos > c2_neg) {
memcpy(R, Ra, 9 * sizeof(double));
t[0] = u3[0]; t[1] = u3[1]; t[2] = u3[2];
} else if (c1_pos < c1_neg && c2_pos < c2_neg) {
memcpy(R, Ra, 9 * sizeof(double));
t[0] = -u3[0]; t[1] = -u3[1]; t[2] = -u3[2];
} else {
//test Rb
P2[0]=Rb[0]; P2[1]=Rb[1]; P2[2]=Rb[2]; P2[3]=u3[0];
P2[4]=Rb[3]; P2[5]=Rb[4]; P2[6]=Rb[5]; P2[7]=u3[1];
P2[8]=Rb[6]; P2[9]=Rb[7]; P2[10]=Rb[8]; P2[11]=u3[2];
cvMatMul(&_K,&_P2,&_P2); //P2 = K2*[Rb,u3];
c1_pos = c1_neg = c2_pos = c2_neg = 0;
for(int i=0; i<(int)p1.size(); ++i) {
if(!inliers[i]) continue;
double X[3];
helper::triangulate(p1[i].x, p1[i].y,
p2[i].x, p2[i].y, P1, P2, X);
double X2[3]={X[0],X[1],X[2]};
CreateCvMatHead(_X2,3,1,X2);
cvMatMul(&_Rb, &_X2, &_X2);
cvAdd(&_X2, &_u3, &_X2);
if (X[2] > 0) c1_pos++;
else c1_neg++;
if (X2[2] > 0) c2_pos++;
else c2_neg++;
}
//cout<<"Test Rb"<<endl;
//cout<<"+c1="<<c1_pos<<"\t-c1="<<c1_neg<<endl;
//cout<<"+c2="<<c2_pos<<"\t-c2="<<c2_neg<<endl;
if (c1_pos > c1_neg && c2_pos > c2_neg) {
memcpy(R, Rb, 9 * sizeof(double));
t[0] = u3[0]; t[1] = u3[1]; t[2] = u3[2];
} else if (c1_pos < c1_neg && c2_pos < c2_neg) {
memcpy(R, Rb, 9 * sizeof(double));
t[0] = -u3[0]; t[1] = -u3[1]; t[2] = -u3[2];
} else {
TagE("no case was found!\n");
return false;
}
};
//final triangulate
double tulen = u3[0]*u3[0]+u3[1]*u3[1]+u3[2]*u3[2];
tulen = sqrt(tulen);
u3[0]*=lamda/tulen; u3[1]*=lamda/tulen; u3[2]*=lamda/tulen;
P2[0]=R[0]; P2[1]=R[1]; P2[2]=R[2]; P2[3]=t[0];
P2[4]=R[3]; P2[5]=R[4]; P2[6]=R[5]; P2[7]=t[1];
P2[8]=R[6]; P2[9]=R[7]; P2[10]=R[8]; P2[11]=t[2];
cvMatMul(&_K,&_P2,&_P2);
if(Log::level>=Log::LOG_DEBUG) helper::print(3,4,P2,"Final P2");
double maxu1=-DBL_MAX,minu1=DBL_MAX,maxv1=-DBL_MAX,minv1=DBL_MAX;
double maxu2=-DBL_MAX,minu2=DBL_MAX,maxv2=-DBL_MAX,minv2=DBL_MAX;
for(int i=0; i<(int)p1.size(); ++i) {
if(!inliers[i]) continue;
double X[3];
helper::triangulate(p1[i].x, p1[i].y,
p2[i].x, p2[i].y, P1, P2, X);
if(X[2]>0)
results.push_back(cv::Point3d(X[0],X[1],X[2]));
double u,v;
helper::project(P1,X[0],X[1],X[2], u,v);
double du1=u-p1[i].x, dv1=v-p1[i].y;
LogD(">P1\t%lf\t%lf\n",du1,dv1);
helper::project(P2,X[0],X[1],X[2], u,v);
double du2=u-p2[i].x, dv2=v-p2[i].y;
LogD(">P2\t%lf\t%lf\n",du2,dv2);
maxu1 = std::max(maxu1,du1); minu1 = std::min(minu1,du1);
maxv1 = std::max(maxv1,dv1); minv1 = std::min(minv1,dv1);
maxu2 = std::max(maxu2,du2); minu2 = std::min(minu2,du2);
maxv2 = std::max(maxv2,dv2); minv2 = std::min(minv2,dv2);
}
TagI("reproject error for img1 = (%g ~ %g, %g ~ %g)\n", minu1, maxu1, minv1, maxv1);
TagI("reproject error for img2 = (%g ~ %g, %g ~ %g)\n", minu2, maxu2, minv2, maxv2);
return true;
}
int CUKF::UnscentedUpdate(int idf, int idx)
{
// Set up some variables
int dim = XX->rows;
int N = 2*dim+1;
double scale = 3;
int kappa = scale-dim;
// Create samples
// SVD
CvSize P = cvGetSize(PX);
CvMat *D = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *U = cvCreateMat(P.height, P.height, CV_64FC1);
CvMat *V = cvCreateMat(P.width, P.width, CV_64FC1);
CvMat *Ds = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *UD = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *Ps = cvCreateMat(P.height, P.width, CV_64FC1);
cvSVD(PX, D, U, V, CV_SVD_V_T);
MatSqrt(D, Ds);
cvMatMul(U, Ds, UD);
cvScale(UD, Ps, sqrt(scale), 0);
// CvMat *ss = cvCreateMat(dim, N, CV_64FC1);
// int i,j,jj;
// int row = dim;
// int col = N;
// cvRepeat(XX, ss);
// // for (i=0; i<row; i++) for (j=0; j<col; j++)
// // ss->data.db[i*col+j] = XXA->data.db[i];
//
// col -= dim;
// for(i=0; i<row; i++) for(j=1, jj=0; j<col+1; j++, jj++)
// ss->data.db[i*col+j] += Ps->data.db[i*col+jj];
// for(i=0; i<row; i++) for(j=1+dim, jj=0; j<col+1+dim; j++, jj++)
// ss->data.db[i*col+j] -= Ps->data.db[i*col+jj];
CvMat *ss = cvCreateMat(dim, N, CV_64FC1);
int i,j,jj;
int row = dim;
int col = N;
cvRepeat(XX, ss);
double *ssdb = ss->data.db;
double *Psdb = Ps->data.db;
for(i=0; i<row; i++)
{
for(j=1, jj=0; j<dim+1; j++, jj++)
{
ssdb[i*ss->cols+j] += Psdb[i*Ps->cols+jj];
}
}
for(i=0; i<row; i++)
{
for(j=1+dim, jj=0; j<ss->cols; j++, jj++)
{
ssdb[i*ss->cols+j] -= Psdb[i*Ps->cols+jj];
}
}
// Transform samples according to observation model to obtain the predicted observation samples
CvMat *zs = cvCreateMat(2, N, CV_64FC1);
// CvMat *base = cvCreateMat(3, EP, CV_64FC1);
int Nxv = 3;
int f = Nxv + idf*2 - 2;
// PrintCvMat(ss, "ss");
double *zsdb = zs->data.db;
for(j=0; j<N; j++)
{
double dx = ssdb[f*ss->cols+j] - ssdb[0*ss->cols+j];
double dy = ssdb[(f+1)*ss->cols+j] - ssdb[1*ss->cols+j];
double d2 = dx*dx + dy*dy;
double d = sqrt(d2);
zsdb[0*zs->cols+j] = d;
zsdb[1*zs->cols+j] = atan2(dy,dx) - ssdb[2*ss->cols+j];
}
// zz = repvec(z,N);
// dz = feval(dzfunc, zz, zs); % compute correct residual
// zs = zz - dz; % offset zs from z according to correct residual
// PrintCvMat(zs, "zs");
CvMat *zz = cvCreateMat(2, N, CV_64FC1);
CvMat *dz = cvCreateMat(2, N, CV_64FC1);
CvMat *zprev = cvCreateMat(2, 1, CV_64FC1);
zprev->data.db[0] = zf[idx].x;
zprev->data.db[1] = zf[idx].y;
cvRepeat(zprev, zz);
cvSub(zz, zs, dz);
double *dzdb = dz->data.db;
for(j=0; j<dz->cols; j++) dzdb[1*dz->cols+j] = PI2PI(dzdb[1*dz->cols+j]);
cvSub(zz, dz, zs);
// PrintCvMat(zs, "zs");
CvMat *zm = cvCreateMat(2, 1, CV_64FC1);
CvMat *dx = cvCreateMat(dim ,N, CV_64FC1);
// CvMat *dz = cvCreateMat(2, N, CV_64FC1);
CvMat *repx = cvCreateMat(dim ,N, CV_64FC1);
CvMat *repzm = cvCreateMat(2, N, CV_64FC1);
CvMat *Pxz = cvCreateMat(dim, 2, CV_64FC1);
CvMat *Pzz = cvCreateMat(2, 2, CV_64FC1);
CvMat *dxu = cvCreateMat(dim, 1, CV_64FC1);
CvMat *dxl = cvCreateMat(dim, N-1, CV_64FC1);
CvMat *dzu = cvCreateMat(2, 1, CV_64FC1);
CvMat *dzl = cvCreateMat(2, N-1, CV_64FC1);
CvMat *dztu = cvCreateMat(1, 2, CV_64FC1);
CvMat *dztl = cvCreateMat(N-1, 2, CV_64FC1);
CvMat *dxdzu = cvCreateMat(dim, 2, CV_64FC1);
CvMat *dxdzl = cvCreateMat(dim, 2, CV_64FC1);
CvMat *dzdzu = cvCreateMat(2, 2, CV_64FC1);
CvMat *dzdzl = cvCreateMat(2, 2, CV_64FC1);
double *zmdb = zm->data.db;
zmdb[0] = kappa*zs->data.db[0*zs->cols];
zmdb[1] = kappa*zs->data.db[1*zs->cols];
for(j=1; j<N; j++)
{
zm->data.db[0] += 0.5*zsdb[0*zs->cols+j];
zm->data.db[1] += 0.5*zsdb[1*zs->cols+j];
}
cvScale(zm, zm, 1/(double)(scale));
// Calculate predicted observation mean
cvRepeat(XX, repx);
cvRepeat(zm, repzm);
// PrintCvMat(ss, "ss");
// PrintCvMat(zs, "zs");
// PrintCvMat(repx, "repx");
// PrintCvMat(repzm, "repzm");
// Calculate observation covariance and the state-observation correlation matrix
cvSub(ss, repx, dx);
cvSub(zs, repzm, dz);
// PrintCvMat(dx, "dx");
// PrintCvMat(dz, "dz");
double *dxdb = dx->data.db;
double *dzudb = dzu->data.db;
double *dzldb = dzl->data.db;
double *dxudb = dxu->data.db;
double *dxldb = dxl->data.db;
// dx, dz를 1열과 나머지 열로 분할
for(i=0; i<2; i++) dzudb[i] = dzdb[i*dz->cols];
for(i=0; i<2; i++) for(j=1, jj=0; j<N; j++, jj++)
dzldb[i*dzl->cols+jj] = dzdb[i*dz->cols+j];
for(i=0; i<dim; i++) dxudb[i] = dxdb[i*dx->cols];
for(i=0; i<dim; i++) for(j=1, jj=0; j<N; j++, jj++)
dxldb[i*dxl->cols+jj] = dxdb[i*dx->cols+j];
cvT(dzu, dztu);
cvT(dzl, dztl);
// PrintCvMat(dxu, "dxu");
// PrintCvMat(dztu, "dztu");
// PrintCvMat(dxl, "dxl");
// PrintCvMat(dztl, "dztl");
cvMatMul(dxu, dztu, dxdzu);
cvScale(dxdzu, dxdzu, 2*kappa);
cvMatMul(dxl, dztl, dxdzl);
cvAdd(dxdzu, dxdzl, Pxz);
cvScale(Pxz, Pxz, 1/(double)(2*scale));
cvMatMul(dzu, dztu, dzdzu);
cvScale(dzdzu, dzdzu, 2*kappa);
cvMatMul(dzl, dztl, dzdzl);
cvAdd(dzdzu, dzdzl, Pzz);
cvScale(Pzz, Pzz, 1/(double)(2*scale));
// PrintCvMat(dx, "dx");
// PrintCvMat(dz, "dz");
// PrintCvMat(dzu, "dzu");
// PrintCvMat(dztu, "dztu");
// PrintCvMat(dzl, "dzl");
// PrintCvMat(dztl, "dztl");
// PrintCvMat(dxu, "dxu");
// PrintCvMat(dxl, "dxl");
// PrintCvMat(dxdzu, "dxdzu");
// PrintCvMat(dxdzl, "dxdzl");
// PrintCvMat(dzdzu, "dzdzu");
// PrintCvMat(dzdzl, "dzdzl");
// PrintCvMat(Pxz, "Pxz");
// PrintCvMat(Pzz, "Pzz");
// Compute Kalman gain
CvMat *S = cvCreateMat(2, 2, CV_64FC1);
CvMat *p = cvCreateMat(2, 2, CV_64FC1);
CvMat *Sct = cvCreateMat(2, 2, CV_64FC1);
CvMat *Sc = cvCreateMat(2, 2, CV_64FC1);
CvMat *Sci = cvCreateMat(2, 2, CV_64FC1);
CvMat *Scit = cvCreateMat(2, 2, CV_64FC1);
CvMat *Wc = cvCreateMat(dim, 2, CV_64FC1);
CvMat *W = cvCreateMat(dim, 2, CV_64FC1);
CvMat *Wz = cvCreateMat(dim, 1, CV_64FC1);
CvMat *Wct = cvCreateMat(2, dim, CV_64FC1);
CvMat *WcWc = cvCreateMat(dim, dim, CV_64FC1);
// % Compute Kalman gain
cvAdd(Pzz, R, S);
//cholesky decomposition이 실패하면 업데이트는 하지 않는다.
if(choldc(S, p, Sct) < 0)
{
TRACE("idf : %d\n", idf);
PrintCvMat(UD, "UD");
PrintCvMat(U, "U");
PrintCvMat(D, "D");
PrintCvMat(V, "V");
PrintCvMat(zprev, "zprev");
PrintCvMat(XX, "XX");
PrintCvMat(PX, "PX");
PrintCvMat(Ps, "Ps");
PrintCvMat(ss, "ss");
for(j=0; j<N; j++)
{
double dx = ss->data.db[f*ss->cols+j] - ss->data.db[0*ss->cols+j];
double dy = ss->data.db[(f+1)*ss->cols+j] - ss->data.db[1*ss->cols+j];
double d2 = dx*dx + dy*dy;
double d = sqrt(d2);
double angle = atan2(dy,dx) - ss->data.db[2*ss->cols+j];
TRACE("%.4f %.4f %.4f %.4f %.4f %.4f %.4f\n", dx, dy, d2, d, angle, atan2(dy, dx), ss->data.db[2*ss->cols+j]);
}
PrintCvMat(zs, "zs");
PrintCvMat(zz, "zz");
PrintCvMat(zm, "zm");
PrintCvMat(dx, "dx");
PrintCvMat(dz, "dz");
PrintCvMat(Pxz, "Pxz");
PrintCvMat(Pzz, "Pzz");
PrintCvMat(R, "R");
return -1;
}
cvT(Sct, Sc);
cvInv(Sc, Sci);
cvT(Sci, Scit);
// PrintCvMat(S, "S");
// PrintCvMat(Sct, "Sct");
// PrintCvMat(Sc, "Sc");
// PrintCvMat(Sci, "Sci");
cvMatMul(Pxz, Sci, Wc);
// PrintCvMat(Wc, "Wc");
cvMatMul(Wc, Scit, W);
// PrintCvMat(W, "W");
cvSub(zprev, zm, zprev);
cvMatMul(W, zprev, Wz);
cvAdd(XX, Wz, XX);
cvT(Wc, Wct);
cvMatMul(Wc, Wct, WcWc);
cvSub(PX, WcWc, PX);
// PrintCvMat(Pzz, "Pzz");
// PrintCvMat(R, "R");
// PrintCvMat(S, "S");
// PrintCvMat(Pxz, "Pxz");
// PrintCvMat(Sc, "Sc");
// PrintCvMat(Sct, "Sct");
// PrintCvMat(Sci, "Sci");
// PrintCvMat(Scit, "Scit");
// PrintCvMat(W, "W");
// PrintCvMat(zprev, "zprev");
// PrintCvMat(zm, "zm");
// PrintCvMat(Wc, "Wc");
// PrintCvMat(Wct, "Wct");
// PrintCvMat(WcWc, "WcWc");
// PrintCvMat(PX, "Px");
//
// PrintCvMat(XX, "XX");
// PrintCvMat(PX, "PX");
cvReleaseMat(&D);
cvReleaseMat(&U);
cvReleaseMat(&V);
cvReleaseMat(&Ds);
cvReleaseMat(&UD);
cvReleaseMat(&Ps);
cvReleaseMat(&ss);
cvReleaseMat(&zs);
cvReleaseMat(&zz);
cvReleaseMat(&dz);
cvReleaseMat(&zprev);
cvReleaseMat(&zm);
cvReleaseMat(&dx);
cvReleaseMat(&repx);
cvReleaseMat(&repzm);
cvReleaseMat(&Pxz);
cvReleaseMat(&Pzz);
cvReleaseMat(&dxu);
cvReleaseMat(&dxl);
cvReleaseMat(&dzu);
cvReleaseMat(&dzl);
cvReleaseMat(&dztu);
cvReleaseMat(&dztl);
cvReleaseMat(&dxdzu);
cvReleaseMat(&dxdzl);
cvReleaseMat(&dzdzu);
cvReleaseMat(&dzdzl);
cvReleaseMat(&S);
cvReleaseMat(&p);
cvReleaseMat(&Sct);
cvReleaseMat(&Sc);
cvReleaseMat(&Sci);
cvReleaseMat(&Scit);
cvReleaseMat(&Wc);
cvReleaseMat(&W);
cvReleaseMat(&Wz);
cvReleaseMat(&Wct);
cvReleaseMat(&WcWc);
return 0;
}
// perform autocalibration using absolute quadric
bool mvg_autocalibration_2(CvMat ** Ps, double * principal_points, const size_t n, CvMat ** Xs, const size_t m, CvMat ** pi_infinity /*= NULL*/, bool affine /*= false*/)
{
if (n < 3)
{
printf("at least three views must be selected\n");
return false;
}
// create deep copy of the input data
CvMat ** Ps_orig = ALLOC(CvMat *, n);
for (size_t i = 0; i < n; i++)
{
Ps_orig[i] = Ps[i];
Ps[i] = opencv_create_matrix(3, 4);
cvCopy(Ps_orig[i], Ps[i]);
}
// move the principal point to the origin for every camera
CvMat * T = opencv_create_matrix(3, 3);
CvMat * S = opencv_create_matrix(3, 3);
for (size_t i = 0; i < n; i++)
{
// set up the translation matrix
cvZero(T);
OPENCV_ELEM(T, 0, 0) = 1;
OPENCV_ELEM(T, 1, 1) = 1;
OPENCV_ELEM(T, 2, 2) = 1;
OPENCV_ELEM(T, 0, 2) = -principal_points[2 * i + 0];
OPENCV_ELEM(T, 1, 2) = -principal_points[2 * i + 1];
// apply it to the projection matrix
cvMatMul(T, Ps[i], Ps[i]);
// also normalize scale
cvZero(S);
OPENCV_ELEM(S, 0, 0) = 0.001;
OPENCV_ELEM(S, 1, 1) = 0.001;
OPENCV_ELEM(S, 2, 2) = 1.0;
cvMatMul(S, Ps[i], Ps[i]);
}
cvReleaseMat(&T);
cvReleaseMat(&S);
// RANSAC paradigm state
const size_t samples = 3;
int best_inliers_count = 0;
size_t best_inliers_ids[samples];
bool
* best_inliers_marked = ALLOC(bool, n),
* inliers_marked = ALLOC(bool, n),
* sample_marked = ALLOC(bool, n)
;
memset(best_inliers_marked, 0, n * sizeof(bool));
memset(best_inliers_ids, 0, samples * sizeof(size_t));
// result
CvMat
* pi_inf = NULL,
* H_rectify = NULL,
* H_rectify_inv = NULL,
* solution = NULL,
* W = NULL
;
// --- begin --- RANSAC paradigm iterator
const int total = 500;
for (int tries = 0; tries <= total; tries++)
{
// --- end --- RANSAC paradigm iterator
// if this is the last RANSAC iteration, we'll use all inliers from the best sample
memset(sample_marked, 0, n * sizeof(bool));
if (tries == total)
{
if (best_inliers_count >= 3)
{
// use all inliers
memcpy(sample_marked, best_inliers_marked, n * sizeof(bool));
printf("Last iteration, using the following sample: ");
for (size_t i = 0; i < n; i++) { printf(sample_marked[i] ? "T" : "F"); }
printf("\n");
}
else
{
// fallback method - use all shots
printf("Failed to find consistent sample when autocalibrating! Using all shots!\n");
memset(sample_marked, ~0, n * sizeof(bool));
best_inliers_count = n;
}
}
else
{
// generate sample
memset(sample_marked, 0, n * sizeof(bool));
for (int count = 0; count < samples;)
{
const int pick = rand() % n;
if (!sample_marked[pick])
{
sample_marked[pick] = true;
count++;
}
}
}
// fill the matrix W containing linear equations determining Q
W = opencv_create_matrix(4 * (tries == total ? best_inliers_count : samples), 10);
cvZero(W);
int i = 0;
for (size_t j = 0; j < n; j++)
{
if (!sample_marked[j]) continue;
// shortcut for P
CvMat * const P = Ps[j];
// (P * Omega * P_t)_12 = 0
OPENCV_ELEM(W, i, 0) = q(P, 1, 2, 0);
OPENCV_ELEM(W, i, 1) = q(P, 1, 2, 1);
OPENCV_ELEM(W, i, 2) = q(P, 1, 2, 2);
OPENCV_ELEM(W, i, 3) = q(P, 1, 2, 3);
OPENCV_ELEM(W, i, 4) = q(P, 1, 2, 4);
OPENCV_ELEM(W, i, 5) = q(P, 1, 2, 5);
OPENCV_ELEM(W, i, 6) = q(P, 1, 2, 6);
OPENCV_ELEM(W, i, 7) = q(P, 1, 2, 7);
OPENCV_ELEM(W, i, 8) = q(P, 1, 2, 8);
OPENCV_ELEM(W, i, 9) = q(P, 1, 2, 9);
i++;
// (P * Omega * P_t)_13 = 0
OPENCV_ELEM(W, i, 0) = q(P, 1, 3, 0);
OPENCV_ELEM(W, i, 1) = q(P, 1, 3, 1);
OPENCV_ELEM(W, i, 2) = q(P, 1, 3, 2);
OPENCV_ELEM(W, i, 3) = q(P, 1, 3, 3);
OPENCV_ELEM(W, i, 4) = q(P, 1, 3, 4);
OPENCV_ELEM(W, i, 5) = q(P, 1, 3, 5);
OPENCV_ELEM(W, i, 6) = q(P, 1, 3, 6);
OPENCV_ELEM(W, i, 7) = q(P, 1, 3, 7);
OPENCV_ELEM(W, i, 8) = q(P, 1, 3, 8);
OPENCV_ELEM(W, i, 9) = q(P, 1, 3, 9);
i++;
// (P * Omega * P_t)_23 = 0
OPENCV_ELEM(W, i, 0) = q(P, 2, 3, 0);
OPENCV_ELEM(W, i, 1) = q(P, 2, 3, 1);
OPENCV_ELEM(W, i, 2) = q(P, 2, 3, 2);
OPENCV_ELEM(W, i, 3) = q(P, 2, 3, 3);
OPENCV_ELEM(W, i, 4) = q(P, 2, 3, 4);
OPENCV_ELEM(W, i, 5) = q(P, 2, 3, 5);
OPENCV_ELEM(W, i, 6) = q(P, 2, 3, 6);
OPENCV_ELEM(W, i, 7) = q(P, 2, 3, 7);
OPENCV_ELEM(W, i, 8) = q(P, 2, 3, 8);
OPENCV_ELEM(W, i, 9) = q(P, 2, 3, 9);
i++;
// (P * Omega * P_t)_11 - (P * Omega * P_t)_22 = 0
OPENCV_ELEM(W, i, 0) = q(P, 1, 1, 0) - q(P, 2, 2, 0);
OPENCV_ELEM(W, i, 1) = q(P, 1, 1, 1) - q(P, 2, 2, 1);
OPENCV_ELEM(W, i, 2) = q(P, 1, 1, 2) - q(P, 2, 2, 2);
OPENCV_ELEM(W, i, 3) = q(P, 1, 1, 3) - q(P, 2, 2, 3);
OPENCV_ELEM(W, i, 4) = q(P, 1, 1, 4) - q(P, 2, 2, 4);
OPENCV_ELEM(W, i, 5) = q(P, 1, 1, 5) - q(P, 2, 2, 5);
OPENCV_ELEM(W, i, 6) = q(P, 1, 1, 6) - q(P, 2, 2, 6);
OPENCV_ELEM(W, i, 7) = q(P, 1, 1, 7) - q(P, 2, 2, 7);
OPENCV_ELEM(W, i, 8) = q(P, 1, 1, 8) - q(P, 2, 2, 8);
OPENCV_ELEM(W, i, 9) = q(P, 1, 1, 9) - q(P, 2, 2, 9);
i++;
}
// solve the system
solution = opencv_right_null_vector(W);
cvReleaseMat(&W);
// construct Q
CvMat * Q_temp = opencv_create_matrix(4, 4);
int si = 0;
for (int i = 0; i < 4; i++)
{
for (int j = i; j < 4; j++)
{
OPENCV_ELEM(Q_temp, i, j) = OPENCV_ELEM(solution, si, 0);
if (i != j) OPENCV_ELEM(Q_temp, j, i) = OPENCV_ELEM(solution, si, 0);
si++;
}
}
cvReleaseMat(&solution);
// SVD decomposition
CvMat * D = opencv_create_matrix(4, 4), * V = opencv_create_matrix(4, 4), * U = opencv_create_matrix(4, 4);
cvSVD(Q_temp, D, U, V, CV_SVD_V_T);
cvReleaseMat(&Q_temp);
OPENCV_ELEM(D, 3, 3) = 0; // Q has rank 3
CvMat * Q_star_inf = opencv_create_matrix(4, 4);
cvZero(Q_star_inf);
cvMatMul(U, D, Q_star_inf);
cvMatMul(Q_star_inf, V, Q_star_inf);
// now find the pi_inf
pi_inf = opencv_right_null_vector(Q_star_inf);
H_rectify = opencv_create_I_matrix(4);
H_rectify_inv = NULL;
if (!affine)
{
// full metric reconstruction
CvMat * E_U = opencv_create_matrix(4, 4), * E_V = opencv_create_matrix(4, 4), * E_D = opencv_create_matrix(4, 1);
cvSVD(Q_star_inf, E_D, E_U, E_V, CV_SVD_V_T);
for (int j = 0; j < 3; j++)
{
for (int i = 0; i < 4; i++)
{
OPENCV_ELEM(E_U, i, j) *= sqrt(OPENCV_ELEM(E_D, j, 0));
}
}
cvInvert(E_U, H_rectify);
H_rectify_inv = E_U;
cvReleaseMat(&E_V);
cvReleaseMat(&E_D);
}
else
{
// affine
for (int i = 0; i < 4; i++) { OPENCV_ELEM(H_rectify, 3, i) = OPENCV_ELEM(pi_inf, i, 0); }
cvInvert(H_rectify, H_rectify_inv);
}
if (tries == total)
{
// apply it to all points
for (int i = 0; i < m; i++)
{
cvMatMul(H_rectify, Xs[i], Xs[i]);
}
// apply it to original matrices
for (int i = 0; i < n; i++)
{
cvMatMul(Ps_orig[i], H_rectify_inv, Ps_orig[i]);
}
}
// now calculate principal points of all the matrices and count inliers
CvMat
* rectified_K = opencv_create_matrix(3, 3),
* rectified_R = opencv_create_matrix(3, 3),
* rectified_T = opencv_create_matrix(3, 1)
;
memset(inliers_marked, 0, n * sizeof(bool));
size_t inliers_count = 0;
CvMat * P_temp = opencv_create_matrix(3, 4);
printf("[");
for (int i = 0; i < n; i++)
{
// at the end of the final iteration, the matrices Ps_orig are already rectified
if (tries < total)
{
cvMatMul(Ps_orig[i], H_rectify_inv, P_temp);
}
else
{
cvCopy(Ps_orig[i], P_temp);
}
const bool decomposed = mvg_finite_projection_matrix_decomposition(P_temp, rectified_K, rectified_R, rectified_T);
if (decomposed)
{
const double PP_distance = sqrt(
sq(OPENCV_ELEM(rectified_K, 0, 2) - principal_points[2 * i + 0]) +
sq(OPENCV_ELEM(rectified_K, 1, 2) - principal_points[2 * i + 1])
);
if (PP_distance < 100)
{
inliers_count++;
inliers_marked[i] = true;
}
else
{
inliers_marked[i] = false;
}
// test
// inliers_marked[i] = sample_marked[i];
printf("%f ", PP_distance);
}
else
{
inliers_marked[i] = false;
printf("failed to extract PP\n");
}
}
cvReleaseMat(&P_temp);
// debug output
printf("] ");
for (size_t i = 0; i < n; i++) { printf(inliers_marked[i] ? "T" : "F"); }
printf(" %d ", inliers_count);
printf("\n");
// set the best result
if (inliers_count > best_inliers_count)
{
memcpy(best_inliers_marked, inliers_marked, n * sizeof(bool));
best_inliers_count = inliers_count;
}
// TODO release allocated matrices
cvReleaseMat(&rectified_K);
cvReleaseMat(&rectified_R);
cvReleaseMat(&rectified_T);
// --- begin --- RANSAC paradigm iterator close
}
printf("\n");
// --- end --- RANSAC paradigm iterator close
// release memory
if (pi_infinity)
{
*pi_infinity = pi_inf;
}
else
{
cvReleaseMat(&pi_inf);
}
for (size_t i = 0; i < n; i++)
{
cvReleaseMat(Ps + i);
}
cvReleaseMat(&H_rectify);
cvReleaseMat(&H_rectify_inv);
cvReleaseMat(&solution);
cvReleaseMat(&W);
return true;
}