Exemple #1
0
/*Compute the inverse of the distance between x and z*/
real invDistance(SparseDim* x, SparseDim* z,real a) {
    if (a == 1.0)
        return (real)(1.0/L2Distance(x,z,0));
    else if(a == 2.0)
        return (real)(1.0/L2Distance(x,z,1));
    else
        return (real)(1.0/pow(L2Distance(x,z,0),a));
}
Exemple #2
0
std::shared_ptr<Matrix> NearestNeighborNoiseEstimation(const Matrix &data) {

    // TODO: Outlier exclusion

    Index d = data.cols();
    Index n = data.rows();

    std::shared_ptr<Matrix> result = std::make_shared<Matrix>(d, d);

    Matrix noise_values(n, d);

    auto l2_dist = L2Distance(data, data, true);

    // Find the second smallest distance (first is 0, and should be forced, so it is smallest non-zero), so no need to fully sort the matrix
    for (Index i = 0; i < n; ++i) {
        Index smallest_non_zero_index = 0;
        Scalar smallest_non_zero_value = (*l2_dist)(i, 0);
        if (i == 0) {
            smallest_non_zero_value = std::numeric_limits<Scalar>::max();
        }
        for (Index j = 1; j < n; ++j) {
            if (j == i) continue;
            if ((*l2_dist)(i, j) < smallest_non_zero_value) {
                smallest_non_zero_index = j;
                smallest_non_zero_value = (*l2_dist)(i, j);
            }
        }

        for (Index k = 0; k < d; ++k) {
            noise_values(i, k) = data(i, k) - data(smallest_non_zero_index, k);
        }
    }

    gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, noise_values.m_, noise_values.m_, 0.0, result->m_);

    return result;
}
Exemple #3
0
/*Compute the polynomial kernel between sparse vectors x and z*/
real gaussianKernel(SparseDim* x, SparseDim* z,real gamma) {
    return (real)exp(-gamma*L2Distance(x,z,1));
}
path *makePath(float startx, float starty, float goalx, float goaly){
	//build the graph, hardcoded and miserable
	writeDebugStream("making path.\n");
  node graph[NUM_WAYPOINTS];
	//bottom row
  graph[0].x = 0.1524; graph[0].y = 0.1524;
	graph[1].x = 0.762;	 graph[1].y = 0.1524;
  graph[2].x = 0.9144; graph[2].y = 0.1524;
	graph[3].x = 2.134;  graph[3].y = 0.1524;
	//top row
	graph[4].x = 0.3048; graph[4].y = 0.9144;
	graph[5].x = 0.9144; graph[5].y = 1.067;
	graph[6].x = 1.372;  graph[6].y = 1.067;
	graph[7].x = 1.676;  graph[7].y = 0.6096;
	graph[8].x = 2.286;  graph[8].y = 0.762;
	graph[9].x = 1.3716; graph[9].y = 0.4572;
	graph[10].x = 1.3716; graph[10].y = 0.1524;
	graph[11].x = 1.676; graph[11].y = 0.1524;
	//set up neighbors. do we need to make the remaining ones NULL?
	graph[0].neighbors[0] = &(graph[1]);
		graph[0].neighbors[1] = NULL; graph[0].neighbors[2] = NULL;	graph[0].neighbors[3] = NULL;		graph[0].neighbors[4] = NULL;
	graph[1].neighbors[0] = &(graph[0]);
	graph[1].neighbors[1] = &(graph[2]);
		graph[1].neighbors[2] = NULL;	graph[1].neighbors[3] = NULL;		graph[1].neighbors[4] = NULL;
	graph[2].neighbors[0] = &(graph[1]);
	graph[2].neighbors[1] = &(graph[9]);
	graph[2].neighbors[2] = &(graph[10]);
		graph[2].neighbors[3] = NULL;		graph[2].neighbors[4] = NULL;
	graph[3].neighbors[0] = &(graph[7]);
	graph[3].neighbors[1] = &(graph[8]);
	graph[3].neighbors[2] = &(graph[11]);
graph[3].neighbors[3] = NULL;		graph[3].neighbors[4] = NULL;


	graph[4].neighbors[0] = &(graph[5]);
	graph[4].neighbors[1] = NULL;		graph[4].neighbors[2] = NULL; graph[4].neighbors[3] = NULL;		graph[4].neighbors[4] = NULL;
	graph[5].neighbors[0] = &(graph[4]);
	graph[5].neighbors[1] = &(graph[6]);
	graph[5].neighbors[2] = NULL;		graph[5].neighbors[3] = NULL; graph[5].neighbors[4] = NULL;
	graph[6].neighbors[0] = &(graph[5]);
	graph[6].neighbors[1] = &(graph[7]);
	graph[6].neighbors[2] = NULL;		graph[6].neighbors[3] = NULL; graph[6].neighbors[4] = NULL;
	graph[7].neighbors[0] = &(graph[6]);
	graph[7].neighbors[1] = &(graph[9]);
	graph[7].neighbors[2] = &(graph[3]);
		graph[7].neighbors[3] = NULL; graph[7].neighbors[4] = NULL;
	graph[8].neighbors[0] = &(graph[3]);
	graph[8].neighbors[1] = NULL;		graph[8].neighbors[2] = NULL; graph[8].neighbors[3] = NULL;		graph[8].neighbors[4] = NULL;
	graph[9].neighbors[0] = &(graph[2]);
	graph[9].neighbors[1] = &(graph[7]);
	graph[9].neighbors[2] = NULL;		graph[9].neighbors[3] = NULL; graph[9].neighbors[4] = NULL;

	graph[10].neighbors[0] = &(graph[2]);
	graph[10].neighbors[1] = NULL;		graph[10].neighbors[2] = NULL; graph[10].neighbors[3] = NULL;		graph[10].neighbors[4] = NULL;
	graph[11].neighbors[0] = &(graph[3]);
	graph[11].neighbors[1] = NULL;		graph[11].neighbors[2] = NULL; graph[11].neighbors[3] = NULL;		graph[11].neighbors[4] = NULL;

	//start and end
	node st;
	node goal;
	st.x = startx;
	st.y = starty;
	goal.x = goalx;
	goal.y = goaly;
	st.neighbors[0] = NULL;
	goal.neighbors[0] = NULL;
writeDebugStream("set up neighbors, except for start and goal.\n");

	//now add start and end- go through waypoints, finding closest valid one.
	//connect the start and end to those points.

 for(int i = 0; i < 1; i++){
		if (st.neighbors[0] == NULL){
			writeDebugStream("set st's initial neighbor. \n");
			st.neighbors[0] = &graph[i];
			graph[i].neighbors[3] = &st;
			writeDebugStream("st.x: %f2 st.y: %f2 stn.x: %f2 stn.y: %f2\n",st.x,st.y,st.neighbors[0]->x,st.neighbors[0]->y);

		}else{
		  	if (L2Distance(&st,&graph[i]) < L2Distance(&st,(st.neighbors[0]))){
				st.neighbors[0] = &(graph[i]);
				graph[i].neighbors[3] = &st;
		  }
		  writeDebugStream("st.x: %f2 st.y: %f2 stn.x: %f2 stn.y: %f2\n",st.x,st.y,st.neighbors[0]->x,st.neighbors[0]->y);

		}

		if (goal.neighbors[0] == NULL){
			writeDebugStream("set goal's initial neighbor. \n");
		  goal.neighbors[0] = &graph[i];
			graph[i].neighbors[4] = &goal;
		}else{
			if (L2Distance(&goal,&graph[i]) < L2Distance(&goal,(goal.neighbors[0]))){
				goal.neighbors[0] = &(graph[i]);
	  		graph[i].neighbors[4] = &goal;
		  }
		}
  }

	writeDebugStream("successfully put in start and goal.\n");
	//now do dfs to find a path. dfs(start,goal)
  //assumption: from path, we get a float * of points to travel to
  path *traveledPath;
  node visitedNodes[14];
  int yay =0;// DFS (traveledPath, st, goal);
  if (!yay) writeDebugStream("DFS unsuccessful! \n");
  //path[0] = st.x; path[1] = st.y;
  //path[2] = goal.x; path[3] = goal.y;

	writeDebugStream("DFS completed.\n");
  return traveledPath;
}