/*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));
}
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;
}
/*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;
}
