//----------------------------------------------------------------------
void weighted_mean(){
double sigma=0,sigma_x=0,sigma_y=0;
for(int i=0;i<PARTICLES;i++){
sigma = 0;
sigma_x = 0;
sigma_y = 0;
for(int j=0;j<PARTICLES;j++){
sigma = sigma + weighting(x[i], x[j], y[i], y[j], 0.2);
sigma_x = sigma_x + weighting(x[i], x[j], y[i], y[j], 0.2) * x[j];
sigma_y = sigma_y + weighting(x[i], x[j], y[i], y[j], 0.2) * y[j];
}
x_weighted_mean[i] = sigma_x /sigma;
y_weighted_mean[i] = sigma_y /sigma;
}
}
//----------------------------------------------------------------------
void calc_covariance_matrix(int i){
double sigma=0,sigma_00=0,sigma_01=0,sigma_10=0,sigma_11=0;
for(int j=0;j<PARTICLES;j++){
sigma = sigma + weighting(x[i], x[j], y[i], y[j], 0.2);
sigma_00 = sigma_00 + weighting(x[i], x[j], y[i], y[j], 0.2) * ( x[j] - x_weighted_mean[i]) * ( x[j] - x_weighted_mean[i]);
sigma_10 = sigma_10 + weighting(x[i], x[j], y[i], y[j], 0.2) * ( x[j] - x_weighted_mean[i]) * ( y[j] - y_weighted_mean[i]);
sigma_01 = sigma_01 + weighting(x[i], x[j], y[i], y[j], 0.2) * ( y[j] - y_weighted_mean[i]) * ( x[j] - x_weighted_mean[i]);
sigma_11 = sigma_11 + weighting(x[i], x[j], y[i], y[j], 0.2) * ( y[j] - y_weighted_mean[i]) * ( y[j] - y_weighted_mean[i]);
}
covariance_matrix[0][0] = sigma_00 / sigma;
covariance_matrix[1][0] = sigma_10 / sigma;
covariance_matrix[0][1] = sigma_01 / sigma;
covariance_matrix[1][1] = sigma_11 / sigma;
}
int main()
{
clock_t beginTime = clock();// clock_t taken as long
double **trainingData, **testData;
int trainingPicNum = 42000;
readInMiniData(&trainingData, trainingPicNum);
// readInData(&trainingData, &testData);
//printData(trainingData, trainingPicNum);
// printf("Well, boy, look at this %d\n", (int)1 == 1.0);
printf("Succefully read in data\n\n");
// Hard coded
double occurrance[10] = {0};
double voting[10][784] = {0};
vote(occurrance, voting, trainingData, trainingPicNum);
double weight[10][784] = {0};
weighting(occurrance, voting, weight);
validate(trainingData, weight, trainingPicNum);
clock_t endTime = clock();
printf("Total time elapsed %lf seconds\n\n", (double)(endTime - beginTime)/(double)CLOCKS_PER_SEC);
return 0;
}
void replace_particle(){
double lambda=0.9;
double sigma=0,sigma_x=0,sigma_y=0;
for(int i=0;i<PARTICLES;i++){
x_smoothing[i] = (1 - lambda) * x[i];
y_smoothing[i] = (1 - lambda) * y[i];
sigma = 0;
sigma_x = 0;
sigma_y = 0;
for(int j=0;j<PARTICLES;j++){
sigma = sigma + weighting(x[i], x[j], y[i], y[j], 0.2);
sigma_x = sigma_x + weighting(x[i], x[j], y[i], y[j], 0.2) * x[j];
sigma_y = sigma_y + weighting(x[i], x[j], y[i], y[j], 0.2) * y[j];
}
x_smoothing[i] = x_smoothing[i] + lambda * sigma_x /sigma;
y_smoothing[i] = y_smoothing[i] + lambda * sigma_y /sigma;
}
}
int huffmanWeight(Tree** heap, int n) {
heapify(heap, n);
while(n > 1) {
Tree* huff =createHuffmanNode();
huff->left = heapDelete(heap, n--);
huff->right = heapDelete(heap, n--);
huff->weight = huff->left->weight + huff->right->weight;
heapAdd(heap, n++, huff);
}
return weighting(heap[0]);
}
// TODO needs some love
void run() {
while(ros::ok()) {
current_time = ros::Time::now();
double dt = (current_time - last_time).toSec();
// TODO these predicst and measure variables are not something that I understand
if (predict) {
prediction(dt);
last_time = current_time;
predict = false;
if (measure) {
update_estimated_measurement();
weighting();
measure = false;
}
Normalization();
resample();
}
if (visualization_enabled) {
if (publish_particles) {
particles_pub.publish(loc_viz.get_particle_marker(particle_state));
}
if (publish_robot) {
robot_pub.publish(
loc_viz.get_robot_marker(std::vector<ras::sensor_info>(), x, y, theta, visualization_use_distance));
}
if (publish_thickened_walls) {
wall_cell_pub.publish(loc_viz.get_thick_wall_grid_cells());
}
if (publish_walls) {
grid_cell_pub.publish(loc_viz.get_wall_grid_cells());
}
if (publish_path) {
point_path_pub.publish(loc_viz.add_to_path(x, y));
}
}
//Publish Geometry msg of Predicted postion
geometry_msgs::Pose2D msg;
msg.x = x;
msg.y = y;
msg.theta = theta;
position_pub.publish(msg);
//Use if add a subscription(Add as good measure)
ros::spinOnce();
//Delays untill it is time to send another message
rate->sleep();
last_time = current_time;
}
}
double kernel_regression(const std::vector<float>& fiber_profile,
const std::vector<double>& inner_angles,
double cur_angle,double sigma)
{
sigma *= sigma;
std::vector<double> weighting(inner_angles.size());
for (unsigned int index = 0; index < inner_angles.size(); ++index)
{
double dx = cur_angle-inner_angles[index];
weighting[index] = std::exp(-dx*dx/2.0/sigma);
}
double result = 0.0;
for (unsigned int index = 0; index < fiber_profile.size(); ++index)
result += fiber_profile[index]*weighting[index];
result /= std::accumulate(weighting.begin(),weighting.end(),0.0);
return result;
}
void darkenate(Colour *background, Colour *foreground, Tile* currentTile, Lightmap* map, WorldCoord worldPoint, Point direction)
{
WorldCoord coord;
double darken = 0.03;
for (int i = -1; i<2; i++) {
for (int j = -1; j < 2; j++) {
coord.X = worldPoint.X - i;
coord.Y = worldPoint.Y - j;
Tile* comparisonTile = map->tileAtPoint(coord);
// Tile* less = NULL;
// Tile* more = NULL;
//direction tells us which is more important
double weight = weighting(direction, i, j);
if (weight == 0) {
continue;
}
if (weight < 0) {
weight = abs(weight);
if (comparisonTile->height > currentTile->height) {
//double val = 1.0/(comparisonTile->height - currentTile->height);
for (int z = 0; z<weight; z++) {
background->darken(darken);
foreground->darken(darken);
}
}
}
else {
//double val = 1.0/(comparisonTile->height - currentTile->height);
if (comparisonTile->height < currentTile->height) {
for (int z = 0; z<weight; z++) {
background->darken(darken);
foreground->darken(darken);
}
}
}
}
}
}
int main() {
int n;
scanf("%d", &n);
Tree* charset[n];
for(int i = 0; i < n; i++) {
charset[i] = (Tree*)malloc(sizeof(Tree));
char str[2];
scanf("%s", str);
scanf("%d", &charset[i]->weight);
charset[i]->key = str[0];
charset[i]->left = NULL;
charset[i]->right = NULL;
}
Tree* tempset[n];
for(int i = 0; i < n; i++) {
tempset[i] = (Tree*)malloc(sizeof(Tree));
copy(charset[i], tempset[i]);
}
int rw = huffmanWeight(tempset, n);
for(int i = 0; i < n; i++) {
free(tempset[i]);
}
int m;
scanf("%d", &m);
for(int i = 0; i < m; i++) { // for each submission
Tree* huffman = createHuffmanNode();
int valid = 1;
int j;
for(j = 0; j < n && valid; j++) { // for each char
char notcare[2];
scanf("%s", notcare);
char code[8];
scanf("%s", code);
Tree* temp = huffman;
for(int k = 0; code[k] != '\0'; k++) {
if(code[k] == '0') {
if(!temp->left) {
temp->left = createHuffmanNode();
}
else {
if(temp->left->key != '*') {
valid = 0;
//printf("node duplicate.\n");
break;
}
}
temp = temp->left;
}
else {
if(!temp->right) {
temp->right = createHuffmanNode();
}
else {
if(temp->right->key != '*') {
valid = 0;
break;
}
}
temp = temp->right;
}
} // end of k
if(temp->left || temp->right) {
valid = 0;
j++;
break;
}
copy(charset[j], temp);
} // end of j
if(j < n) {
for(j; j < n; j++) {
char notcare[2];
scanf("%s", notcare);
char code[8];
scanf("%s", code);
}
} // read unused inputs
if(valid) { // what's valid? All chars are Huffman's leaves.
// traversal(huffman);
int aw = weighting(huffman);
if(aw == rw) {
printf("Yes\n");
}
else {
printf("No\n");
}
}
else {
printf("No\n");
}
} // end of i
return 0;
}
