// compute optimal pose and scale using a procedure described by Umeyame,
// Least-squares estimation of transformation parameters between two point patterns (IEEE Pami 1991)
double computeScalingFactor(MeshType* mesh1, MeshType* mesh2)
{
if (mesh1->GetNumberOfPoints() != mesh2->GetNumberOfPoints()) {
throw std::runtime_error("meshes should be in correspondence when computing specificity");
}
PointListType points1;
PointListType points2;
for (unsigned i = 0; i < mesh1->GetNumberOfPoints(); i++) {
MeshType::PointType pt1;
mesh1->GetPoint(i, &pt1);
points1.push_back(pt1);
MeshType::PointType pt2;
mesh2->GetPoint(i, &pt2);
points2.push_back(pt2);
}
vnlMatrixType X = createMatrixFromPoints(points1);
vnlMatrixType Y = createMatrixFromPoints(points2);
vnlVectorType mu_x = calcMean(X);
vnlVectorType mu_y = calcMean(Y);
ElementType sigma2_x = calcVar(X, mu_x);
ElementType sigma2_y = calcVar(Y, mu_y);
vnlMatrixType Sigma_xy = calcCov(X, Y, mu_x, mu_y);
vnl_svd<ElementType> SVD(Sigma_xy);
unsigned m = X.rows();
vnlMatrixType S(m,m);
S.set_identity();
ElementType detU = vnl_qr<ElementType>(SVD.U()).determinant();
ElementType detV = vnl_qr<ElementType>(SVD.V()).determinant();
if ( detU * detV == -1) {
S[m-1][m-1] = -1;
}
// the procedure actually computes the optimal rotation, translation and scale. We only
// use the scaling factor
vnlMatrixType R = SVD.U() * S * SVD.V().transpose();
ElementType c = 1/sigma2_x * vnl_trace(SVD.W()*S);
vnlVectorType t = mu_y - c * R * mu_x;
ElementType epsilon2 = sigma2_y - pow(vnl_trace(SVD.W()*S), 2) / sigma2_x;
return c;
}
int main(){
//printf("hello world!!!!!!!! WOOOOOOO!! \n");
// Calculate overhead of reading TIME
int numTrials = 100000;
int i=0;
double mean;
double stDev;
unsigned long long results[numTrials];
unsigned long long beg, end;
for(i=0; i<numTrials; i++){
beg = rdtsc();
end = rdtsc();
results[i] = end-beg;
}
mean = calcMean(results, numTrials);
stDev = calcStDev(results, numTrials);
//printf("%lld \n", end-beg);
printf("Time Mean: %0.2f, Time Standard Deviation: %0.2f \n", mean, stDev);
return 0;
}
void doWorker()
{
printf("[%s.%d] worker started\n", processor_name, my_rank);
// begin parallel code; fill in the missing code here
MPI_Status status;
int master_id = 0;
int i = 0;
int n = 0;
int start = 0;
MPI_Recv(&n, 1, MPI_INT, master_id, 0, MPI_COMM_WORLD, &status);
int *array = (int*) malloc(sizeof(int) * n);
MPI_Recv(array, n, MPI_INT, master_id, 0, MPI_COMM_WORLD, &status);
int size = 6;
int *newArray = (int*) malloc(sizeof(int) * n);
newArray[0] = calcSum(array, n);
newArray[1] = calcMean(array, n);
newArray[2] = findMax(array, n);
newArray[3] = findMin(array, n);
MPI_Send(&n, 1, MPI_INT, worker_id, 0, MPI_COMM_WORLD);
MPI_Send(&array[start], n, MPI_INT, worker_id, 0, MPI_COMM_WORLD);
// end parallel code; no more code change required
printf("[%s.%d] worker completed\n", processor_name, my_rank);
}
static void processAccelBias(BiasObj* bias)
{
Axis3f mean;
calcMean(bias, &mean);
varianceSampleTime = xTaskGetTickCount();
bias->value.x = (int16_t)(mean.x + 0.5f);
bias->value.y = (int16_t)(mean.y + 0.5f);
bias->value.z = (int16_t)(mean.z + 0.5f);
bias->found = 1;
}
double calcStd(int *array, int N)
{
int i = 0;
double ss = 0.0, mean = calcMean(array, N);
for (i = 0; i < N; i++) {
ss += (mean - array[i]) * (mean - array[i]);
}
return sqrt(ss/N);
}
/**
* Compute the RMS value of a REAL4Vector
* \param [out] rms Pointer to the output RMS value
* \param [in] vector Pointer to a REAL4Vector of values
* \return Status value
*/
INT4 calcRms(REAL4 *rms, REAL4Vector *vector)
{
REAL4Vector *sqvector = NULL;
XLAL_CHECK( (sqvector = XLALCreateREAL4Vector(vector->length)) != NULL, XLAL_EFUNC );
sqvector = XLALSSVectorMultiply(sqvector, vector, vector);
XLAL_CHECK( xlalErrno == 0, XLAL_EFUNC );
*rms = sqrtf(calcMean(sqvector));
XLALDestroyREAL4Vector(sqvector);
return XLAL_SUCCESS;
} /* calcRms() */
float IntegralImage::calcVariance(Rect r)
{
int width=_integral.cols;
int height=_integral.rows;
int a=r.x+(r.y*width);
int b=(r.x+r.width)+(r.y*width);
int c=r.x+((r.y+r.height)*width);
int d=(r.x+r.width)+(r.y+r.height)*width;
float mx=calcMean(r);
double *ii2 = (double *)_sq_integral.data;
float mx2=ii2[a]+ii2[d]-ii2[b]-ii2[c];
mx2=mx2/(r.width*r.height);
mx2=mx2-(mx*mx);
return mx2;
}
int main()
{
int EV_seed;
int EV_num;
int EV_mean;
struct EV_linkedNums * EV_nums;
scanf("%d", &EV_seed);
scanf("%d", &EV_num);
EV_nums = getRands(EV_seed, EV_num);
EV_mean = calcMean(EV_nums);
printf("%d\n",EV_mean);
range(EV_nums);
approxSqrtAll(EV_nums);
return 0;
}
float calcKurtosis(float * array, int size)
{
float mean, variance, diff_sum = 0.0;
int i;
mean = calcMean(array, size);
variance = calcVariance(array, size);
variance = pow( variance, 0.5 );
for(i = 0; i < size; i++)
{
diff_sum += pow( (array[i] - mean) / variance, 4);
}
return (diff_sum / size) - 3;
}
/* runs the program */
int main(int argc, char ** argv) {
char i_name[MAXNAME + 1] ; /* name of current entry in file */
int i_grade ; /* grade of current entry in file */
int i; /* counter */
/* file i/o to read in file for grades */
FILE *inputF ;
char *mode = "r" ;
/* store the file handle */
inputF = fopen(argv[1], mode) ;
/* if cannot read file */
if(inputF == NULL) {
fprintf(stderr, "Invalid input file.") ;
exit(1) ;
}
/* pass in the address of "grade" */
while(fscanf(inputF, "%s %d", i_name, &i_grade) != EOF) {
i_name[0] = toupper(i_name[0]) ;
struct grade_entry entry ;
entry.name = calloc( strlen( i_name ) + 1, sizeof( char ) ) ;
strncpy(entry.name, i_name, strlen( i_name ) ) ;
entry.grade = i_grade ;
grade_list[numgrades] = entry ;
numgrades++ ;
}
printf("\nUnsorted data: \n");
print_grades();
printf("\nSort by name: \n");
sort_by_name();
print_grades();
printf("\nSort by grade: \n");
sort_by_grade();
print_grades();
printf("\nMean: %d\n", calcMean(grade_list, numgrades));
printf("Median: %d\n", calcMedian(grade_list, numgrades));
return 0;
}
/**
* Calculates the variance and mean for the bias buffer.
*/
static void calcVarianceAndMean(BiasObj* bias, Axis3f* variance, Axis3f* mean)
{
Axis3i16* elem;
int64_t sumSquared[GYRO_NBR_OF_AXES] = {0};
for (elem = bias->bufStart;
elem != (bias->bufStart+SENSORS_NBR_OF_BIAS_SAMPLES); elem++)
{
sumSquared[0] += elem->x * elem->x;
sumSquared[1] += elem->y * elem->y;
sumSquared[2] += elem->z * elem->z;
}
calcMean(bias, mean);
variance->x = fabs(sumSquared[0] / SENSORS_NBR_OF_BIAS_SAMPLES
- mean->x * mean->x);
variance->y = fabs(sumSquared[1] / SENSORS_NBR_OF_BIAS_SAMPLES
- mean->y * mean->y);
variance->z = fabs(sumSquared[2] / SENSORS_NBR_OF_BIAS_SAMPLES
- mean->z * mean->z);
}
/**
* \brief Compute the harmonic mean value of a REAL4Vector of SFT values
*
* The harmonic mean is computed from the mean values of the SFTs
* \param [out] harmonicMean Pointer to the output REAL4 harmonic mean value
* \param [in] vector Pointer to a REAL4Value from which to compute the harmonic mean
* \param [in] numfbins Number of frequency bins in the SFTs
* \param [in] numffts Number of SFTs during the observation time
* \return Status value
*/
INT4 calcHarmonicMean(REAL4 *harmonicMean, REAL4Vector *vector, INT4 numfbins, INT4 numffts)
{
INT4 values = 0;
REAL4Vector *tempvect = NULL;
XLAL_CHECK( (tempvect = XLALCreateREAL4Vector(numfbins)) != NULL, XLAL_EFUNC );
for (INT4 ii=0; ii<numffts; ii++) {
if (vector->data[ii*numfbins]!=0.0) {
memcpy(tempvect->data, &(vector->data[ii*numfbins]), sizeof(REAL4)*numfbins);
*harmonicMean += 1.0/calcMean(tempvect);
values++;
}
}
if (values>0) *harmonicMean = (REAL4)values/(*harmonicMean);
else *harmonicMean = 0.0;
XLALDestroyREAL4Vector(tempvect);
return XLAL_SUCCESS;
} /* calcHarmonicMean() */
void doManager(int n, int z, int seed)
{
int N = n * n;
int *matrix = createArray(N, z);
printf("[%s.%d] manager started\n", processor_name, my_rank);
// begin parallel code; fill in the missing code here
// double mean = 0.0, sum = 0.0, std = 0.0, max = 0.0, min = 0.0;
MPI_Status status;
int start, worker_id, worker_count = world_size - 1;
if (N % worker_count == 0) {
int start = 0;
MPI_Send(&N, 1, MPI_INT, worker_id, 0, MPI_COMM_WORLD);
MPI_Send(&matrix[start], N, MPI_INT, worker_id, 0, MPI_COMM_WORLD);
}
else{
int start = 0;
MPI_Send(&N, 1, MPI_INT, worker_id, 0, MPI_COMM_WORLD);
MPI_Send(&matrix[start], N, MPI_INT, worker_id, 0, MPI_COMM_WORLD);
}
int count = world_size;
// end parallel code; no more code change required
printf("[%s.%d] manager completed\n", processor_name, my_rank);
sleep(1);
printArray(matrix, n);
printf("N = %d\n", N);
printf("sum = %g %g\n", sum, calcSum(matrix, N));
printf("mean = %g %g\n", mean, calcMean(matrix, N));
printf("std = %g %g\n", std, calcStd(matrix, N));
printf("max = %g %g\n", max, findMax(matrix, N));
printf("min = %g %g\n", min, findMin(matrix, N));
}
//-----------------------------------------------------------------------------------------
void makePlot(TH1 *h, const char *title, int pad, float Ymin, float Ymax, float YlimitB, float YlimitC) {
int rescale = 0;
double mean, rms, y(0.76);
c1_4->cd(pad);
gPad->SetLeftMargin(0.15);
for (int i = startChip; i < startChip+nChips; i++) {
if ( h->GetBinContent(i+1) > Ymax ) {
Ymax = h->GetBinContent(i+1)+0.2* h->GetBinContent(i+1);
}
if ( h->GetBinContent(i+1) > YlimitC ) {
rescale = 1;
}
if ( h->GetBinContent(i+1) < Ymin ) {
Ymin = h->GetBinContent(i+1)-0.2* h->GetBinContent(i+1);
}
}
h->GetYaxis()->SetRangeUser(Ymin, Ymax);
h->Draw("LP");
line->DrawLine(startChip,YlimitB,startChip+nChips,YlimitB);
if ( rescale ) {
line->DrawLine(startChip,YlimitC,startChip+nChips,YlimitC);
}
mean = calcMean(h);
rms = calcRMS(h, mean);
tl->DrawLatex(0.2, 0.93, Form("%s", title));
char title2[200];
if (pad != 3) { sprintf(title2, "%s [e]", title); }
else { sprintf(title2, "%s [\%]", title); mean = 100*mean; rms = 100*rms; }
y -= 0.11*(pad-1);
c1_3->cd(3);
tl->DrawLatex(0.25, y, Form("%s",title2));
// tl->DrawLatex(0.72, y, Form("%.1f +- %.1f", mean, rms));
tl->DrawLatex(0.72, y, Form("%.1f", mean));
}
//-----------------------------------------------------------------------------------------
int grading(int *rocs, double i150, double ratio, TH1D *h1[], TH1D *h2[],
float *criteriaB, float *criteriaC, const char *testNr) {
double value(0.), nValue(0.);
int gr(1);
int jbin(0);
for (int i = 0; i < 4; i++) {
for (int j = startChip; j < startChip+nChips; j++) {
jbin = j - startChip;
value = h1[i]->GetBinContent(jbin+1);
nValue = h2[i]->GetBinContent(jbin+1);
// Grading
if ( (gr == 1) && (value > criteriaB[i]) ) { gr = 2; }
if ( (value > criteriaC[i]) ) { gr = 3; }
if ( (gr == 1) && (nValue < (4160 - defectsB)) ) { gr = 2; }
if ( (nValue < (4160 - defectsC)) ) { gr = 3; }
// Failures reasons...
if ( (value > criteriaB[i]) && (value < criteriaC[i]) ) { fitsProblemB[i]++; }
if ( (value > criteriaC[i]) ) { fitsProblemC[i]++; }
}
}
if ( !strcmp(testNr,"T+17a") ) {
// Grading
if( (gr == 1) && (rocs[1] > 0 ) ) { gr = 2; }
if( (gr == 1) && (i150 > currentB) ) { gr = 2; }
if( (gr == 1) && (ratio > slopeivB) ) { gr = 2; }
if( (rocs[2] > 0 ) ) { gr = 3; }
if( (i150 > currentC) ) { gr = 3; }
// Failures reasons...
if( (i150 > currentB) && (i150 < currentC) ) { currentProblemB++; }
if( (i150 > currentC) ) { currentProblemC++;}
if( (ratio > slopeivB) ) { slopeProblemB++;}
}
if ( !strcmp(testNr,"T-10b") || !strcmp(testNr,"T-10a") ) {
// Grading
if( (gr == 1) && (rocs[1] > 0 ) ) { gr = 2; }
if( (gr == 1) && (i150 > 1.5*currentB) ) { gr = 2; }
if( (gr == 1) && (ratio > slopeivB ) ) { gr = 2; }
if( (rocs[2] > 0 ) ) { gr = 3; }
if( (i150 > 1.5*currentC) ) { gr = 3; }
// Failures reasons...
if( (i150 > 1.5*currentB) && (i150 < 1.5*currentC) ) { currentProblemB++; }
if( (i150 > 1.5*currentC) ) { currentProblemC++;}
if( (ratio > slopeivB ) ) { slopeProblemB++;}
}
return gr;
}
glm::mat4
RGBDSensor::guess_eye_d_to_world_static(const ChessboardSampling& cbs, const Checkerboard& cb){
if(cbs.getIRs().empty() || cbs.getPoses().empty()){
std::cerr << "ERROR in RGBDSensor::guess_eye_d_to_world_static: no Chessboard found" << std::endl;
exit(0);
}
glm::mat4 chessboard_pose = cb.pose_offset * cbs.getPoses()[0].mat;
ChessboardViewIR cbvir(cbs.getIRs()[0]);
std::vector<glm::vec3> exs;
std::vector<glm::vec3> eys;
const float u = cbvir.corners[0].x;
const float v = cbvir.corners[0].y;
const float d = cbvir.corners[0].z;
std::cerr << "calc origin at " << u << ", " << v << ", " << d << std::endl;
glm::vec3 origin = calc_pos_d(u,v,d);
for(unsigned i = 1; i < (CB_WIDTH * CB_HEIGHT); ++i){
const float u = cbvir.corners[i].x;
const float v = cbvir.corners[i].y;
const float d = cbvir.corners[i].z;
glm::vec3 corner = calc_pos_d(u,v,d);
if(i < CB_WIDTH){
eys.push_back(glm::normalize(corner - origin));
}
else if(i % CB_WIDTH == 0){
exs.push_back(glm::normalize(corner - origin));
}
}
glm::vec3 ex(calcMean(exs));
glm::vec3 ey(calcMean(eys));
ex = glm::normalize(ex);
ey = glm::normalize(ey);
glm::vec3 ez = glm::cross(ex,ey);
ey = glm::cross(ez,ex);
std::cerr << "origin: " << origin << std::endl;
std::cerr << "ex: " << ex << std::endl;
std::cerr << "ey: " << ey << std::endl;
std::cerr << "ez: " << ez << std::endl;
glm::mat4 eye_d_to_world;
eye_d_to_world[0][0] = ex[0];
eye_d_to_world[0][1] = ex[1];
eye_d_to_world[0][2] = ex[2];
eye_d_to_world[1][0] = ey[0];
eye_d_to_world[1][1] = ey[1];
eye_d_to_world[1][2] = ey[2];
eye_d_to_world[2][0] = ez[0];
eye_d_to_world[2][1] = ez[1];
eye_d_to_world[2][2] = ez[2];
eye_d_to_world[3][0] = origin[0];
eye_d_to_world[3][1] = origin[1];
eye_d_to_world[3][2] = origin[2];
eye_d_to_world = glm::inverse(eye_d_to_world);
return chessboard_pose * eye_d_to_world;
}
glm::mat4
RGBDSensor::guess_eye_d_to_world(const ChessboardSampling& cbs, const Checkerboard& cb){
// find slowest ChessboardPose
const double time = cbs.searchSlowestTime(cbs.searchStartIR());
bool valid_pose;
glm::mat4 chessboard_pose = cb.pose_offset * cbs.interpolatePose(time,valid_pose);
if(!valid_pose){
std::cerr << "ERROR: could not interpolate valid pose" << std::endl;
exit(0);
}
// find pose of that board in kinect camera space
bool valid_ir;
ChessboardViewIR cbvir(cbs.interpolateIR(time, valid_ir));
if(!valid_ir){
std::cerr << "ERROR: could not interpolate valid ChessboardIR" << std::endl;
exit(0);
}
std::vector<glm::vec3> exs;
std::vector<glm::vec3> eys;
const float u = cbvir.corners[0].x;
const float v = cbvir.corners[0].y;
const float d = cbvir.corners[0].z;
std::cerr << "calc origin at " << u << ", " << v << ", " << d << std::endl;
glm::vec3 origin = calc_pos_d(u,v,d);
for(unsigned i = 1; i < (CB_WIDTH * CB_HEIGHT); ++i){
const float u = cbvir.corners[i].x;
const float v = cbvir.corners[i].y;
const float d = cbvir.corners[i].z;
glm::vec3 corner = calc_pos_d(u,v,d);
if(i < CB_WIDTH){
eys.push_back(glm::normalize(corner - origin));
}
else if(i % CB_WIDTH == 0){
exs.push_back(glm::normalize(corner - origin));
}
}
glm::vec3 ex(calcMean(exs));
glm::vec3 ey(calcMean(eys));
ex = glm::normalize(ex);
ey = glm::normalize(ey);
glm::vec3 ez = glm::cross(ex,ey);
ey = glm::cross(ez,ex);
std::cerr << "origin: " << origin << std::endl;
std::cerr << "ex: " << ex << std::endl;
std::cerr << "ey: " << ey << std::endl;
std::cerr << "ez: " << ez << std::endl;
glm::mat4 eye_d_to_world;
eye_d_to_world[0][0] = ex[0];
eye_d_to_world[0][1] = ex[1];
eye_d_to_world[0][2] = ex[2];
eye_d_to_world[1][0] = ey[0];
eye_d_to_world[1][1] = ey[1];
eye_d_to_world[1][2] = ey[2];
eye_d_to_world[2][0] = ez[0];
eye_d_to_world[2][1] = ez[1];
eye_d_to_world[2][2] = ez[2];
eye_d_to_world[3][0] = origin[0];
eye_d_to_world[3][1] = origin[1];
eye_d_to_world[3][2] = origin[2];
eye_d_to_world = glm::inverse(eye_d_to_world);
return chessboard_pose * eye_d_to_world;
}
