static carmen_vector_3D_t
get_velodyne_point_car_reference(double rot_angle, double vert_angle, double range, carmen_pose_3D_t velodyne_pose, carmen_pose_3D_t sensor_board_pose, rotation_matrix* velodyne_to_board_matrix, rotation_matrix* board_to_car_matrix)
{
double cos_rot_angle = cos(rot_angle);
double sin_rot_angle = sin(rot_angle);
double cos_vert_angle = cos(vert_angle);
double sin_vert_angle = sin(vert_angle);
double xy_distance = range * cos_vert_angle;
carmen_vector_3D_t velodyne_reference;
velodyne_reference.x = (xy_distance * cos_rot_angle);
velodyne_reference.y = (xy_distance * sin_rot_angle);
velodyne_reference.z = (range * sin_vert_angle);
carmen_vector_3D_t board_reference = multiply_matrix_vector(velodyne_to_board_matrix, velodyne_reference);
board_reference = add_vectors(board_reference, velodyne_pose.position);
carmen_vector_3D_t car_reference = multiply_matrix_vector(board_to_car_matrix, board_reference);
car_reference = add_vectors(car_reference, sensor_board_pose.position);
return car_reference;
}
void rbm::cd_single_data(float * features_idx, float ** weights, float * hidden_bias, float * visible_bias, float ** delta_weights,
float * delta_hidden_bias, float * delta_visible_bias, int K){
float * v = new float[num_visible_units];
float * h = new float[num_hidden_units];
float * p_h = new float[num_hidden_units];
float * p_v = new float[num_visible_units];
float * p_0 = new float[num_hidden_units];
float * wvc = new float[num_hidden_units];
float * whb = new float[num_visible_units];
copy_vec(v, features_idx, num_visible_units);
// compute wv0
multiply_matrix_vector(weights, v, wvc, num_hidden_units, num_visible_units);
// compute wvc0
add_vector(wvc, hidden_bias, num_hidden_units);
// compute p_0
compute_probability_hidden_given_visible(wvc, p_0, num_hidden_units);
// do K-step gibbs sampling
for (int i=0; i<K; i++){
// compute wv
multiply_matrix_vector(weights, v, wvc, num_hidden_units, num_visible_units);
// compute wvc
add_vector(wvc, hidden_bias, num_hidden_units);
compute_probability_hidden_given_visible(wvc, p_h, num_hidden_units);
sample_vector(h, p_h, num_hidden_units);
// compute wh
multiply_matrix_vector_column_wise(weights, h, whb, num_hidden_units, num_visible_units);
// compute whb
add_vector(whb, visible_bias, num_visible_units);
compute_probability_visible_given_hidden(whb, p_v, num_visible_units);
sample_vector(v, p_v, num_visible_units);
}
update_weights_gradient(delta_weights, features_idx, v, p_0, p_h, num_hidden_units, num_visible_units);
update_visible_bias_gradient(delta_visible_bias, features_idx, v, num_visible_units);
update_hidden_bias_gradient(delta_hidden_bias, p_0, p_h, num_hidden_units);
delete[] v;
delete[] h;
delete[] p_h;
delete[] p_v;
delete[] p_0;
delete[] wvc;
delete[] whb;
}
int main(int argc, char** argv) {
double *v, *ws, *wp, no;
int l;
int n = atoi(argv[1]);
int num_th = atoi(argv[2]);
// check whether command line arguments are correct
if (n < num_th){
printf("num_th has to be smaller or equal than n\n");
return 1;
}
if (n % num_th != 0){
printf("n has to be a multiple of num_th\n");
return 1;
}
l = n/num_th;
// allocate memory for A according to C99 standard (variabel size array)
double (*A)[n] = malloc(n*n*sizeof(double));
v = (double*) malloc(n*sizeof(double));
wp = (double*) malloc(n*sizeof(double));
ws = (double*) malloc(n*sizeof(double));
// fill A and v with random numbers
fill_random_matrix(n, n, A);
fill_random_vector(v, n);
multiply_matrix_vector(n, A, v, ws);
# pragma omp parallel num_threads(num_th)
multiply_matrix_vector_para(n, A, v, wp, l);
no = norm(ws,wp,n);
printf("norm=%f\n",no);
free(v);
free(ws);
free(wp);
free(A);
return 0;
}
int main(int argc, char** argv) {
int n=5;
double *v, *ws, *wp, no;
/* Matrix A and vector v are to be muliplied
The solution is ws for sequential multiplication and
wp for parallel multiplication
*/
// allocate memory for A according to C99 standard (variabel size array)
double (*A)[n] = malloc(n*n*sizeof(double));
v = (double*) malloc(n*sizeof(double));
wp = (double*) malloc(n*sizeof(double));
ws = (double*) malloc(n*sizeof(double));
// fill A and v with random numbers
fill_random_matrix(n, n, A);
fill_random_vector(v, n);
multiply_matrix_vector(n, A, v, ws);
// calculate the norm of ws - wp
// The parallel algorithm works correct if no == 0;
no = norm(ws,wp,n);
printf("norm=%f\n",no);
free(v);
free(ws);
free(wp);
free(A);
return 0;
}
static carmen_vector_3D_t
get_xsens_position_global_reference (carmen_pose_3D_t xsens_pose, carmen_pose_3D_t sensor_board_pose, carmen_pose_3D_t car_pose)
{
rotation_matrix* board_to_car_matrix = create_rotation_matrix (sensor_board_pose.orientation);
rotation_matrix* car_to_global_matrix = create_rotation_matrix (car_pose.orientation);
carmen_vector_3D_t car_reference = multiply_matrix_vector (board_to_car_matrix, xsens_pose.position);
car_reference = add_vectors (car_reference, sensor_board_pose.position);
carmen_vector_3D_t global_reference = multiply_matrix_vector (car_to_global_matrix, car_reference);
global_reference = add_vectors (global_reference, car_pose.position);
destroy_rotation_matrix (board_to_car_matrix);
destroy_rotation_matrix (car_to_global_matrix);
return global_reference;
}
carmen_vector_3D_t
carmen_change_sensor_reference(carmen_vector_3D_t position, carmen_vector_3D_t reference, rotation_matrix* transformation_matrix)
{
carmen_vector_3D_t new_reference = multiply_matrix_vector(transformation_matrix, reference);
carmen_vector_3D_t point = add_vectors(new_reference, position);
return point;
}
int main(int argc, char** argv) {
int i;
int n = atoi(argv[1]);
int num_th = atoi(argv[2]);
double *v, *ws, *wp, no;
// check whether input is correct
if (n < num_th){
printf("num_th has to be smaller or equal than n\n");
return 1;
}
if (n % num_th != 0){
printf("n has to be a multiple of num_th\n");
return 1;
}
// lines to calculate per thread
int l = n / num_th;
// allocate memory for A according to C99 standard (variabel size array)
double (*A)[n] = malloc(n*n*sizeof(double));
v = (double*) malloc(n*sizeof(double));
wp = (double*) malloc(n*sizeof(double));
ws = (double*) malloc(n*sizeof(double));
thread_parm_t *parm[n];
pthread_t thread[n];
fill_random_matrix(n, n, A);
fill_random_vector(v, n);
// calculate A * v sequentially as a reference
multiply_matrix_vector(n, A, v, ws);
// for each thread one parameter list is creadted
for (i=0; i<num_th; i++){
parm[i] = malloc(sizeof(thread_parm_t));
parm[i]->matrix=A;
parm[i]->vec=v;
parm[i]->result=wp;
parm[i]->dimension=n;
parm[i]->row=i;
parm[i]->lines=l;
}
// create all threads
for (i=0; i<num_th; i++){
pthread_create(&thread[i], NULL, multiply_matrix_vector_para, (void *)parm[i]);
}
//wait for all threads to complete
for (i=0; i<num_th; i++){
pthread_join(thread[i],NULL);
}
// calcualte norm of wp - ws
no = norm(wp,ws,n);
printf("norm=%f\n",no);
free(v);
free(ws);
free(wp);
free(A);
return 0;
}
void
move_camera (carmen_vector_3D_t displacement)
{
rotation_matrix* r_matrix = create_rotation_matrix (camera_pose.orientation);
carmen_vector_3D_t displacement_world_coordinates = multiply_matrix_vector (r_matrix, displacement);
camera_pose.position = add_vectors (camera_pose.position, displacement_world_coordinates);
destroy_rotation_matrix (r_matrix);
}
static void
transform_pad_pin_data(ElementPadPinData* ppd, int len,
Matrix3x3 trans)
{
int i;
for (i = 0; i < len; i++) {
Vector3x1 v;
point_to_vec(ppd[i].center, v);
Vector3x1 t;
multiply_matrix_vector(trans, v, t);
ppd[i].transformed_center = vec_to_point(t);
}
}
static carmen_vector_3D_t
get_point_position_global_reference(carmen_vector_3D_t car_position, carmen_vector_3D_t car_reference, rotation_matrix* car_to_global_matrix)
{
//rotation_matrix* r_matrix = create_rotation_matrix(car_fused_pose.orientation);
carmen_vector_3D_t global_reference = multiply_matrix_vector(car_to_global_matrix, car_reference);
carmen_vector_3D_t point = add_vectors(global_reference, car_position);
//destroy_rotation_matrix(r_matrix);
return point;
}
int main(int argc, char** argv) {
int n, m;
double **A, *v, *ws, *wp, no;
n = 5;
m= 5;
A=make_matrix(n, m);
v=make_vector(n);
wp=make_vector(n);
ws=make_vector(n);
A=fill_random_matrix(A, n, m);
v=fill_random_vector(v, n);
ws=multiply_matrix_vector(A, v, ws, n, m);
wp=mulitply_matrix_vector_para(A, v, wp, n, m);
no = norm(ws,wp,n);
printf("norm=%f\n",no);
return 0;
}
// compute hidden nodes given model parameters and for a new visible input
void compute_hidden(float ** weights, float * hidden_bias, float * visible, float * hidden, int num_hidden_units, int num_visible_units){
multiply_matrix_vector(weights, visible, hidden, num_hidden_units, num_visible_units);
add_vector(hidden, hidden_bias, num_hidden_units);
activate_logistic(hidden, num_hidden_units);
}
void
draw_ldmrs_objects(carmen_laser_ldmrs_object *ldmrs_objects_tracking, int num_ldmrs_objects, double min_velocity)
{
int i;
rotation_matrix *rotate = NULL;
for (i = 0; i < num_ldmrs_objects; i++)
{
if (ldmrs_objects_tracking[i].velocity < min_velocity)
continue;
carmen_pose_3D_t pos;
carmen_vector_3D_t p1, p2, p3 , p4, p5, p6, p7, p8, t;
carmen_vector_3D_t s1, s2, s3, s4; /* moving object direction arrow */
double correction_wheel_height = 0.28;
double W, L, H;
pos.position.x = ldmrs_objects_tracking[i].x;
pos.position.y = ldmrs_objects_tracking[i].y;
pos.position.z = 0.0;
pos.orientation.yaw = ldmrs_objects_tracking[i].orientation;
pos.orientation.roll = 0.0;
pos.orientation.pitch = 0.0;
W = ldmrs_objects_tracking[i].width;
L = ldmrs_objects_tracking[i].lenght;
H = 1.0;
// W = moving_objects_tracking[i].width;
// L = moving_objects_tracking[i].length;
// H = moving_objects_tracking[i].height;
rotate = compute_rotation_matrix(NULL, pos.orientation);
glColor3d(0.0,1.0,1.0);
p1.x = - L/2.0;
p1.y = - W/2.0;
p1.z = 0.0 - correction_wheel_height;
p2.x = L/2.0;
p2.y = - W/2.0;
p2.z = 0.0 - correction_wheel_height;
p3.x = L/2.0;
p3.y = W/2.0;
p3.z = 0.0 - correction_wheel_height;
p4.x = - L/2.0;
p4.y = W/2.0;
p4.z = 0.0 - correction_wheel_height;
p5.x = - L/2.0;
p5.y = - W/2.0;
p5.z = H - correction_wheel_height;
p6.x = L/2.0;
p6.y = - W/2.0;
p6.z = H - correction_wheel_height;
p7.x = L/2.0;
p7.y = W/2.0;
p7.z = H - correction_wheel_height;
p8.x = - L/2.0;
p8.y = W/2.0;
p8.z = H - correction_wheel_height;
t = multiply_matrix_vector(rotate, p1);
p1 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p2);
p2 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p3);
p3 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p4);
p4 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p5);
p5 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p6);
p6 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p7);
p7 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p8);
p8 = add_vectors(t, pos.position);
glBegin (GL_LINES);
glVertex3d (p1.x, p1.y, p1.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p1.x, p1.y, p1.z);
//////////////////////////////
glVertex3d (p5.x, p5.y, p5.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p8.x, p8.y, p8.z);
glVertex3d (p8.x, p8.y, p8.z);
glVertex3d (p5.x, p5.y, p5.z);
//////////////////////////////
glVertex3d (p1.x, p1.y, p1.z);
glVertex3d (p5.x, p5.y, p5.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p8.x, p8.y, p8.z);
glEnd ();
/* Moving Object direction arrow */
s1.x = 0.0;
s1.y = 0.0;
s1.z = H - correction_wheel_height;
s2.x = L/2.0;
s2.y = 0.0;
s2.z = H - correction_wheel_height;
s3.x = (L/2.0) - 0.3;
s3.y = 0.2;
s3.z = H - correction_wheel_height;
s4.x = (L/2.0) - 0.3;
s4.y = -0.2;
s4.z = H - correction_wheel_height;
t = multiply_matrix_vector(rotate, s1);
s1 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s2);
s2 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s3);
s3 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s4);
s4 = add_vectors(t, pos.position);
glBegin(GL_LINES);
/* Part of arrow: | */
glVertex3d(s1.x, s1.y, s1.z);
glVertex3d(s2.x, s2.y, s2.z);
/* Part of arrow: / */
glVertex3d(s3.x, s3.y, s3.z);
glVertex3d(s2.x, s2.y, s2.z);
/* Part of arrow: \ */
glVertex3d(s4.x, s4.y, s4.z);
glVertex3d(s2.x, s2.y, s2.z);
glEnd();
//center of object
glPushAttrib(GL_POINT_BIT);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(s1.x, s1.y, s1.z);
glEnd();
glPopAttrib();
// char class_name[50];
//
// switch (ldmrs_objects_tracking[i].classId)
// {
// case 0:
// strcpy(class_name,"Unclassified");
// break;
// case 1:
// strcpy(class_name,"Small");
// break;
// case 2:
// strcpy(class_name,"Big");
// break;
// case 3:
// strcpy(class_name,"Pedestrian");
// break;
// case 4:
// strcpy(class_name,"Bike");
// break;
// case 5:
// strcpy(class_name,"Car");
// break;
// case 6:
// strcpy(class_name,"Truck");
// break;
// default:
// strcpy(class_name,"Unknown");
// break;
// }
/* has to drawn after stuff above, so that it appears on top */
//draw_number_associated(pos.position.x, pos.position.y, ldmrs_objects_tracking[i].id,"");
draw_linear_velocity(pos.position.x, pos.position.y, ldmrs_objects_tracking[i].velocity,
1.0);
destroy_rotation_matrix(rotate);
}
}
// forward activation
void rbm::forward_activation(float ** weights, float * hidden_bias, float * visible, float * hidden){
multiply_matrix_vector(weights, visible, hidden, num_hidden_units, num_visible_units);
add_vector(hidden, hidden_bias, num_hidden_units);
activate_logistic(hidden, num_hidden_units);
}
void
draw_tracking_moving_objects(moving_objects_tracking_t *moving_objects_tracking, int current_num_point_clouds,
carmen_vector_3D_t offset, int draw_particles_flag)
{
/*** MOVING OBJECTS MODULE ***/
int i;
rotation_matrix *rotate = NULL;
for (i = 0; i < current_num_point_clouds; i++)
{
if (moving_objects_tracking[i].geometric_model == -1)
continue;
carmen_pose_3D_t pos;
pos = moving_objects_tracking[i].moving_objects_pose;
carmen_vector_3D_t p1, p2, p3 , p4, p5, p6, p7, p8, t;
carmen_vector_3D_t s1, s2, s3, s4; /* moving object direction arrow */
double correction_wheel_height = 0.28;
double W, L, H;
pos.position.x = pos.position.x - offset.x;
pos.position.y = pos.position.y - offset.y;
pos.position.z = 0.0;
W = moving_objects_tracking[i].model_features.geometry.width;
L = moving_objects_tracking[i].model_features.geometry.length;
H = moving_objects_tracking[i].model_features.geometry.height;
// W = moving_objects_tracking[i].width;
// L = moving_objects_tracking[i].length;
// H = moving_objects_tracking[i].height;
rotate = compute_rotation_matrix(NULL, pos.orientation);
glColor3d(moving_objects_tracking[i].model_features.red,
moving_objects_tracking[i].model_features.green,
moving_objects_tracking[i].model_features.blue);
p1.x = - L/2.0;
p1.y = - W/2.0;
p1.z = 0.0 - correction_wheel_height;
p2.x = L/2.0;
p2.y = - W/2.0;
p2.z = 0.0 - correction_wheel_height;
p3.x = L/2.0;
p3.y = W/2.0;
p3.z = 0.0 - correction_wheel_height;
p4.x = - L/2.0;
p4.y = W/2.0;
p4.z = 0.0 - correction_wheel_height;
p5.x = - L/2.0;
p5.y = - W/2.0;
p5.z = H - correction_wheel_height;
p6.x = L/2.0;
p6.y = - W/2.0;
p6.z = H - correction_wheel_height;
p7.x = L/2.0;
p7.y = W/2.0;
p7.z = H - correction_wheel_height;
p8.x = - L/2.0;
p8.y = W/2.0;
p8.z = H - correction_wheel_height;
t = multiply_matrix_vector(rotate, p1);
p1 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p2);
p2 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p3);
p3 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p4);
p4 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p5);
p5 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p6);
p6 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p7);
p7 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p8);
p8 = add_vectors(t, pos.position);
glBegin (GL_LINES);
glVertex3d (p1.x, p1.y, p1.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p1.x, p1.y, p1.z);
//////////////////////////////
glVertex3d (p5.x, p5.y, p5.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p8.x, p8.y, p8.z);
glVertex3d (p8.x, p8.y, p8.z);
glVertex3d (p5.x, p5.y, p5.z);
//////////////////////////////
glVertex3d (p1.x, p1.y, p1.z);
glVertex3d (p5.x, p5.y, p5.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p8.x, p8.y, p8.z);
glEnd ();
/* Moving Object direction arrow */
s1.x = 0.0;
s1.y = 0.0;
s1.z = H - correction_wheel_height;
s2.x = L/2.0;
s2.y = 0.0;
s2.z = H - correction_wheel_height;
s3.x = (L/2.0) - 0.3;
s3.y = 0.2;
s3.z = H - correction_wheel_height;
s4.x = (L/2.0) - 0.3;
s4.y = -0.2;
s4.z = H - correction_wheel_height;
t = multiply_matrix_vector(rotate, s1);
s1 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s2);
s2 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s3);
s3 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s4);
s4 = add_vectors(t, pos.position);
glBegin(GL_LINES);
/* Part of arrow: | */
glVertex3d(s1.x, s1.y, s1.z);
glVertex3d(s2.x, s2.y, s2.z);
/* Part of arrow: / */
glVertex3d(s3.x, s3.y, s3.z);
glVertex3d(s2.x, s2.y, s2.z);
/* Part of arrow: \ */
glVertex3d(s4.x, s4.y, s4.z);
glVertex3d(s2.x, s2.y, s2.z);
glEnd();
//center of object
glPushAttrib(GL_POINT_BIT);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(s1.x, s1.y, s1.z);
glEnd();
glPopAttrib();
/* has to drawn after stuff above, so that it appears on top */
draw_number_associated(pos.position.x, pos.position.y, moving_objects_tracking[i].num_associated,
moving_objects_tracking[i].model_features.model_name);
draw_linear_velocity(pos.position.x, pos.position.y, moving_objects_tracking[i].linear_velocity,
moving_objects_tracking[i].model_features.geometry.height);
destroy_rotation_matrix(rotate);
if (draw_particles_flag == 1) {
// //todo para visualizar as particulas apenas
// for(j = 0; j < 10; j++){
// carmen_pose_3D_t pos;
// pos = moving_objects_tracking[i].moving_objects_pose;
// carmen_vector_3D_t p1, p2, p3 , p4, p5, p6, p7, p8, t;
// carmen_vector_3D_t s1, s2, s3, s4; /* moving object direction arrow */
// double correction_wheel_height = 0.28;
// double W, L, H;
//
// pos.position.x = moving_objects_tracking[i].particulas[j].pose.x - offset.x;
// pos.position.y = moving_objects_tracking[i].particulas[j].pose.y - offset.y;
// pos.position.z = 0.0;
// pos.orientation.yaw = moving_objects_tracking[i].particulas[j].pose.theta;
//
// W = moving_objects_tracking[i].particulas[j].geometry.width;
// L = moving_objects_tracking[i].particulas[j].geometry.length;
// H = moving_objects_tracking[i].particulas[j].geometry.height;
//
// // W = moving_objects_tracking[i].width;
// // L = moving_objects_tracking[i].length;
// // H = moving_objects_tracking[i].height;
//
// rotate = compute_rotation_matrix(NULL, pos.orientation);
//
// glColor3d(moving_objects_tracking[i].model_features.red -0.5,
// moving_objects_tracking[i].model_features.green -0.5,
// moving_objects_tracking[i].model_features.blue -0.5);
//
// p1.x = - L/2.0;
// p1.y = - W/2.0;
// p1.z = 0.0 - correction_wheel_height;
//
// p2.x = L/2.0;
// p2.y = - W/2.0;
// p2.z = 0.0 - correction_wheel_height;
//
// p3.x = L/2.0;
// p3.y = W/2.0;
// p3.z = 0.0 - correction_wheel_height;
//
// p4.x = - L/2.0;
// p4.y = W/2.0;
// p4.z = 0.0 - correction_wheel_height;
//
// p5.x = - L/2.0;
// p5.y = - W/2.0;
// p5.z = H - correction_wheel_height;
//
// p6.x = L/2.0;
// p6.y = - W/2.0;
// p6.z = H - correction_wheel_height;
//
// p7.x = L/2.0;
// p7.y = W/2.0;
// p7.z = H - correction_wheel_height;
//
// p8.x = - L/2.0;
// p8.y = W/2.0;
// p8.z = H - correction_wheel_height;
//
// t = multiply_matrix_vector(rotate, p1);
// p1 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p2);
// p2 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p3);
// p3 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p4);
// p4 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p5);
// p5 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p6);
// p6 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p7);
// p7 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p8);
// p8 = add_vectors(t, pos.position);
//
// glBegin (GL_LINES);
//
// glVertex3d (p1.x, p1.y, p1.z);
// glVertex3d (p2.x, p2.y, p2.z);
//
// glVertex3d (p2.x, p2.y, p2.z);
// glVertex3d (p3.x, p3.y, p3.z);
//
// glVertex3d (p3.x, p3.y, p3.z);
// glVertex3d (p4.x, p4.y, p4.z);
//
// glVertex3d (p4.x, p4.y, p4.z);
// glVertex3d (p1.x, p1.y, p1.z);
// //////////////////////////////
//
// glVertex3d (p5.x, p5.y, p5.z);
// glVertex3d (p6.x, p6.y, p6.z);
//
// glVertex3d (p6.x, p6.y, p6.z);
// glVertex3d (p7.x, p7.y, p7.z);
//
// glVertex3d (p7.x, p7.y, p7.z);
// glVertex3d (p8.x, p8.y, p8.z);
//
// glVertex3d (p8.x, p8.y, p8.z);
// glVertex3d (p5.x, p5.y, p5.z);
// //////////////////////////////
//
// glVertex3d (p1.x, p1.y, p1.z);
// glVertex3d (p5.x, p5.y, p5.z);
//
// glVertex3d (p2.x, p2.y, p2.z);
// glVertex3d (p6.x, p6.y, p6.z);
//
// glVertex3d (p3.x, p3.y, p3.z);
// glVertex3d (p7.x, p7.y, p7.z);
//
// glVertex3d (p4.x, p4.y, p4.z);
// glVertex3d (p8.x, p8.y, p8.z);
//
// glEnd ();
//
// /* Moving Object direction arrow */
// s1.x = 0.0;
// s1.y = 0.0;
// s1.z = H - correction_wheel_height;
//
// s2.x = L/2.0;
// s2.y = 0.0;
// s2.z = H - correction_wheel_height;
//
// s3.x = (L/2.0) - 0.3;
// s3.y = 0.2;
// s3.z = H - correction_wheel_height;
//
// s4.x = (L/2.0) - 0.3;
// s4.y = -0.2;
// s4.z = H - correction_wheel_height;
//
// t = multiply_matrix_vector(rotate, s1);
// s1 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, s2);
// s2 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, s3);
// s3 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, s4);
// s4 = add_vectors(t, pos.position);
//
// glBegin(GL_LINES);
// /* Part of arrow: | */
// glVertex3d(s1.x, s1.y, s1.z);
// glVertex3d(s2.x, s2.y, s2.z);
//
// /* Part of arrow: / */
// glVertex3d(s3.x, s3.y, s3.z);
// glVertex3d(s2.x, s2.y, s2.z);
//
// /* Part of arrow: \ */
// glVertex3d(s4.x, s4.y, s4.z);
// glVertex3d(s2.x, s2.y, s2.z);
//
// glEnd();
//
// //center of object
// glPushAttrib(GL_POINT_BIT);
// glPointSize(5);
// glBegin(GL_POINTS);
// glVertex3d(s1.x, s1.y, s1.z);
// glEnd();
// glPopAttrib();
//
// /* has to drawn after stuff above, so that it appears on top */
// // draw_number_associated(pos.position.x, pos.position.y, moving_objects_tracking[i].num_associated,
// // moving_objects_tracking[i].model_features.model_name);
//// draw_linear_velocity(pos.position.x, pos.position.y, moving_objects_tracking[i].particulas[j].velocity,
//// moving_objects_tracking[i].particulas[j].geometry.height);
//
// destroy_rotation_matrix(rotate);
// }
}
}
}
int main(int argc,char **argv)
{
char stdi=0;
double *pm;
long i,j;
FILE *file;
double **mat,**inverse,*vec,**coeff,**diff,avpm;
if (scan_help(argc,argv))
show_options(argv[0]);
scan_options(argc,argv);
#ifndef OMIT_WHAT_I_DO
if (verbosity&VER_INPUT)
what_i_do(argv[0],WID_STR);
#endif
infile=search_datafile(argc,argv,NULL,verbosity);
if (infile == NULL)
stdi=1;
if (outfile == NULL) {
if (!stdi) {
check_alloc(outfile=(char*)calloc(strlen(infile)+4,(size_t)1));
strcpy(outfile,infile);
strcat(outfile,".ar");
}
else {
check_alloc(outfile=(char*)calloc((size_t)9,(size_t)1));
strcpy(outfile,"stdin.ar");
}
}
if (!stdo)
test_outfile(outfile);
if (column == NULL)
series=(double**)get_multi_series(infile,&length,exclude,&dim,"",dimset,
verbosity);
else
series=(double**)get_multi_series(infile,&length,exclude,&dim,column,
dimset,verbosity);
check_alloc(my_average=(double*)malloc(sizeof(double)*dim));
set_averages_to_zero();
if (poles >= length) {
fprintf(stderr,"It makes no sense to have more poles than data! Exiting\n");
exit(AR_MODEL_TOO_MANY_POLES);
}
check_alloc(vec=(double*)malloc(sizeof(double)*poles*dim));
check_alloc(mat=(double**)malloc(sizeof(double*)*poles*dim));
for (i=0;i<poles*dim;i++)
check_alloc(mat[i]=(double*)malloc(sizeof(double)*poles*dim));
check_alloc(coeff=(double**)malloc(sizeof(double*)*dim));
inverse=build_matrix(mat);
for (i=0;i<dim;i++) {
build_vector(vec,i);
coeff[i]=multiply_matrix_vector(inverse,vec);
}
check_alloc(diff=(double**)malloc(sizeof(double*)*dim));
for (i=0;i<dim;i++)
check_alloc(diff[i]=(double*)malloc(sizeof(double)*length));
pm=make_residuals(diff,coeff);
if (stdo) {
avpm=pm[0]*pm[0];
for (i=1;i<dim;i++)
avpm += pm[i]*pm[i];
avpm=sqrt(avpm/dim);
printf("#average forcast error= %e\n",avpm);
printf("#individual forecast errors: ");
for (i=0;i<dim;i++)
printf("%e ",pm[i]);
printf("\n");
for (i=0;i<dim*poles;i++) {
printf("# ");
for (j=0;j<dim;j++)
printf("%e ",coeff[j][i]);
printf("\n");
}
if (!run_model || (verbosity&VER_USR1)) {
for (i=poles;i<length;i++) {
if (run_model)
printf("#");
for (j=0;j<dim;j++)
if (verbosity&VER_USR2)
printf("%e %e ",series[j][i]+my_average[j],diff[j][i]);
else
printf("%e ",diff[j][i]);
printf("\n");
}
}
if (run_model && (ilength > 0))
iterate_model(coeff,pm,NULL);
}
else {
file=fopen(outfile,"w");
if (verbosity&VER_INPUT)
fprintf(stderr,"Opened %s for output\n",outfile);
avpm=pm[0]*pm[0];
for (i=1;i<dim;i++)
avpm += pm[i]*pm[i];
avpm=sqrt(avpm/dim);
fprintf(file,"#average forcast error= %e\n",avpm);
fprintf(file,"#individual forecast errors: ");
for (i=0;i<dim;i++)
fprintf(file,"%e ",pm[i]);
fprintf(file,"\n");
for (i=0;i<dim*poles;i++) {
fprintf(file,"# ");
for (j=0;j<dim;j++)
fprintf(file,"%e ",coeff[j][i]);
fprintf(file,"\n");
}
if (!run_model || (verbosity&VER_USR1)) {
for (i=poles;i<length;i++) {
if (run_model)
fprintf(file,"#");
for (j=0;j<dim;j++)
if (verbosity&VER_USR2)
fprintf(file,"%e %e ",series[j][i]+my_average[j],diff[j][i]);
else
fprintf(file,"%e ",diff[j][i]);
fprintf(file,"\n");
}
}
if (run_model && (ilength > 0))
iterate_model(coeff,pm,file);
fclose(file);
}
if (outfile != NULL)
free(outfile);
if (infile != NULL)
free(infile);
free(vec);
for (i=0;i<poles*dim;i++) {
free(mat[i]);
free(inverse[i]);
}
free(mat);
free(inverse);
for (i=0;i<dim;i++) {
free(coeff[i]);
free(diff[i]);
}
free(coeff);
free(diff);
free(pm);
return 0;
}
static int computeMatrixLMSOpt(TAsoc *cp_ass, int cnt, Tsc *estimacion) {
int i;
float LMETRICA2;
float X1[MAXLASERPOINTS], Y1[MAXLASERPOINTS];
float X2[MAXLASERPOINTS],Y2[MAXLASERPOINTS];
float X2Y2[MAXLASERPOINTS],X1X2[MAXLASERPOINTS];
float X1Y2[MAXLASERPOINTS], Y1X2[MAXLASERPOINTS];
float Y1Y2[MAXLASERPOINTS];
float K[MAXLASERPOINTS], DS[MAXLASERPOINTS];
float DsD[MAXLASERPOINTS], X2DsD[MAXLASERPOINTS], Y2DsD[MAXLASERPOINTS];
float Bs[MAXLASERPOINTS], BsD[MAXLASERPOINTS];
float A1, A2, A3, B1, B2, B3, C1, C2, C3, D1, D2, D3;
MATRIX matA,invMatA;
VECTOR vecB,vecSol;
A1=0;A2=0;A3=0;B1=0;B2=0;B3=0;
C1=0;C2=0;C3=0;D1=0;D2=0;D3=0;
LMETRICA2=params.LMET*params.LMET;
for (i=0; i<cnt; i++){
X1[i]=cp_ass[i].nx*cp_ass[i].nx;
Y1[i]=cp_ass[i].ny*cp_ass[i].ny;
X2[i]=cp_ass[i].rx*cp_ass[i].rx;
Y2[i]=cp_ass[i].ry*cp_ass[i].ry;
X2Y2[i]=cp_ass[i].rx*cp_ass[i].ry;
X1X2[i]=cp_ass[i].nx*cp_ass[i].rx;
X1Y2[i]=cp_ass[i].nx*cp_ass[i].ry;
Y1X2[i]=cp_ass[i].ny*cp_ass[i].rx;
Y1Y2[i]=cp_ass[i].ny*cp_ass[i].ry;
K[i]=X2[i]+Y2[i] + LMETRICA2;
DS[i]=Y1Y2[i] + X1X2[i];
DsD[i]=DS[i]/K[i];
X2DsD[i]=cp_ass[i].rx*DsD[i];
Y2DsD[i]=cp_ass[i].ry*DsD[i];
Bs[i]=X1Y2[i]-Y1X2[i];
BsD[i]=Bs[i]/K[i];
A1=A1 + (1-Y2[i]/K[i]);
B1=B1 + X2Y2[i]/K[i];
C1=C1 + (-cp_ass[i].ny + Y2DsD[i]);
D1=D1 + (cp_ass[i].nx - cp_ass[i].rx -cp_ass[i].ry*BsD[i]);
A2=B1;
B2=B2 + (1-X2[i]/K[i]);
C2=C2 + (cp_ass[i].nx-X2DsD[i]);
D2=D2 + (cp_ass[i].ny -cp_ass[i].ry +cp_ass[i].rx*BsD[i]);
A3=C1;
B3=C2;
C3=C3 + (X1[i] + Y1[i] - DS[i]*DS[i]/K[i]);
D3=D3 + (Bs[i]*(-1+DsD[i]));
}
initialize_matrix(&matA,3,3);
MDATA(matA,0,0)=A1; MDATA(matA,0,1)=B1; MDATA(matA,0,2)=C1;
MDATA(matA,1,0)=A2; MDATA(matA,1,1)=B2; MDATA(matA,1,2)=C2;
MDATA(matA,2,0)=A3; MDATA(matA,2,1)=B3; MDATA(matA,2,2)=C3;
if (inverse_matrix (&matA, &invMatA)==-1)
return -1;
#ifdef INTMATSM_DEB
print_matrix("inverted matrix", &invMatA);
#endif
initialize_vector(&vecB,3);
VDATA(vecB,0)=D1; VDATA(vecB,1)=D2; VDATA(vecB,2)=D3;
multiply_matrix_vector (&invMatA, &vecB, &vecSol);
estimacion->x=-VDATA(vecSol,0);
estimacion->y=-VDATA(vecSol,1);
estimacion->tita=-VDATA(vecSol,2);
return 1;
}
