code: private double score(double betas[]) {
  		UtilLepetitEPnP.computeCameraControl(betas,nullPts, solutionPts,numControl);
  
  		int index = 0;
  		double score = 0;
  		for( int i = 0; i < numControl; i++ ) {
  			Point3D_F64 si = solutionPts.get(i);
  			Point3D_F64 wi = controlWorldPts.get(i);
  
  			for( int j = i+1; j < numControl; j++ , index++) {
  				double ds = si.distance(solutionPts.get(j));
  				double dw = wi.distance(controlWorldPts.get(j));
  
  				score += (ds-dw)*(ds-dw);
  			}
  		}
  		return score;
  	}
docs: Score a solution based on distance between control points.  Closer the camera
  control points are from the world control points the better the score.  This is
  similar to how optimization score works and not the way recommended in the original
  paper.