How to fit a line to points in OpenCV

In the next few days we will post some functions which are usefull for some applications which need calibration through points. The functions use OpenCV. Unfortunateley it is the old version of OpenCV without C++ support but it should be easy to throw some namespaces in and get C++ methods out of the C functions.
We did not spend much time in optimising the code for speed because there was no need to. But also this drawback might be overcome by changing the set functions for matrixes to pointer operations.
Lets see what we have. We will start with a function which is able to calculate a line with minimum distance to the given points. What we get from the input points is a base point and a direction of the line.

/**
 * Calculates a base point and a direction for a given set of input points.
 */
void cvFindLineFromPoints(CvPoint3D32f **points, int numPoints, CvMat *point, 
  CvMat *direction)
{
    CvMat** inputMatrix = new CvMat*[numPoints];
    for (int i = 0; i < numPoints; i++)
    {
        inputMatrix[i]=cvCreateMat(3, 1, point->type);
        cvmSet(inputMatrix[i], 0, 0, points[i]->x);
        cvmSet(inputMatrix[i], 1, 0, points[i]->y);
        cvmSet(inputMatrix[i], 2, 0, points[i]->z);
    }
    cvFindLineFromPoints(inputMatrix, numPoints, point, direction);
}

void cvFindLineFromPoints(CvMat **inputMatrix, int numPoints, CvMat *point, 
  CvMat *direction)
{
    //---------------------------------------------------------------------------------------
    // Approximate a line through a few points in space
    //---------------------------------------------------------------------------------------

    CvMat *covarMatrix=cvCreateMat(3, 3, point->type);
    CvMat *avgArray=cvCreateMat(3, 1, point->type);

/* Covarianze matrix which is 3x3 actually. */
    cvCalcCovarMatrix((const void **)inputMatrix, numPoints, covarMatrix, avgArray, 
      CV_COVAR_NORMAL);
/* For results equal to Matlab we have to apply a scaling factor which is always 
   1/(N - 1). */
    cvScale(covarMatrix, covarMatrix, 1/((double)(numPoints - 1)));

#ifdef DEBUG_OUTPUT
    for (int row = 0; row < covarMatrix->rows; row++)
    {
        double* ptr = (double*)(covarMatrix->data.ptr + row * covarMatrix->step);
        for (int col = 0; col < covarMatrix->cols; col++)
        {
            std::cout << *ptr++ << " ";
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;
    std::cout << std::endl;
    std::cout << "Point: " << cvmGet(avgArray,0,0) << " " << cvmGet(avgArray,1,0) 
      << " " << cvmGet(avgArray,2,0) << std::endl;
#endif
    cvmSet(point, 0, 0, cvmGet(avgArray, 0, 0));
    cvmSet(point, 1, 0, cvmGet(avgArray, 1, 0));
    cvmSet(point, 2, 0, cvmGet(avgArray, 2, 0));

//** Eigenvectors and Eigenvalues in decreasing order... */
    CvMat* E  = cvCreateMat(3,3,point->type);
    CvMat* l  = cvCreateMat(3,1,point->type);
    cvEigenVV(covarMatrix, E, l);

#ifdef DEBUG_OUTPUT
    for (int row = 0; row < E->rows; row++)
    {
        const double* ptr = (const double*)(E->data.ptr + row * E->step);
        for (int col = 0; col < E->cols; col++)
        {
            std::cout << *ptr++ << " ";
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;
    std::cout << std::endl;
#endif

    CvMat *start = cvCreateMat(3, 1, point->type);
    CvMat *end = cvCreateMat(3, 1, point->type);
    CvMat *_dir = cvCreateMat(3, 1, point->type);

    cvmSet(start, 0, 0, (cvmGet(avgArray, 0, 0) - sqrt(cvmGet(l, 0, 0)) * cvmGet(E, 0, 0)));
    cvmSet(start, 1, 0, (cvmGet(avgArray, 1, 0) - sqrt(cvmGet(l, 0, 0)) * cvmGet(E, 0, 1)));
    cvmSet(start, 2, 0, (cvmGet(avgArray, 2, 0) - sqrt(cvmGet(l, 0, 0)) * cvmGet(E, 0, 2)));

    cvmSet(end, 0, 0, (cvmGet(avgArray, 0, 0) + sqrt(cvmGet(l, 0, 0)) * cvmGet(E, 0, 0)));
    cvmSet(end, 1, 0, (cvmGet(avgArray, 1, 0) + sqrt(cvmGet(l, 0, 0)) * cvmGet(E, 0, 1)));
    cvmSet(end, 2, 0, (cvmGet(avgArray, 2, 0) + sqrt(cvmGet(l, 0, 0)) * cvmGet(E, 0, 2)));

    cvSub(end, start, _dir);
    cvNormalize(_dir, _dir);

#ifdef DEBUG_OUTPUT
    std::cout << "Dir: " << cvmGet(_dir,0,0) << " " << cvmGet(_dir,1,0) << " " 
      << cvmGet(_dir,2,0) << std::endl;
    std::cout << std::endl;
    std::cout << std::endl;
#endif
    cvmSet(direction, 0, 0, cvmGet(_dir, 0, 0));
    cvmSet(direction, 1, 0, cvmGet(_dir, 1, 0));
    cvmSet(direction, 2, 0, cvmGet(_dir, 2, 0));

    cvFree(&covarMatrix);
    cvFree(&avgArray);
    cvFree(&start);
    cvFree(&end);
    cvFree(&_dir);
    cvFree(&E);
    cvFree(&l);
}

As one can see the eigenvector corresponding to the highes eigenvalue gives the direction of the line. The quality of the estimation can also be seen in the difference to the other eigenvalues. If one eigenvalue is significant higher then the others the estimation can be taken as good.