void check_planes(t_ray ray, t_list *planes, t_intersectInfo **o, \ double *max) { t_intersectInfo *i; while (planes) { i = plane_intersection(((t_plane *)planes->data), ray); if (i && i->t > 0 && i->t < *max) { *max = i->t; free(*o); *o = i; } else free(i); planes = planes->next; } }
// // Measurement inverse. Given a measurement ( {x,y} pixel coordinates ) and pose calculate the // world coordinates of the feature int Measurement_Inverse(IplImage *img, int px, int py, float focal_length, float pitch, float roll, float alt, CvMat *feature_point_res) { struct line l; struct plane pl; float plane_point[] = { 0, 0, alt, 1 }; float tmps[] = { px, py, focal_length, 1 }; float def_bear[] = { 0, 1, 0, 0 }; CvMat pix, def_bear_mat; cvInitMatHeader(&def_bear_mat, 4, 1, CV_32FC1, &def_bear); cvInitMatHeader(&pix, 4, 1, CV_32FC1, &tmps); // Init line cvInitMatHeader(&(l.p0), 4, 1, CV_32FC1, line_p0); // Init plane cvInitMatHeader(&(pl.p), 4, 1, CV_32FC1, plane_point); cvInitMatHeader(&(pl.norm), 4, 1, CV_32FC1, plane_norm); // Convert the pixels to meters units pixelTometer(img, &pix); // Rotate with pitch and roll yRotate(&pix, roll, &pix); xRotate(&pix, pitch, &pix); l.p1 = &pix; // Check for no solution to plane intersection if ( !plane_intersection(&pl, &l, &pix) ) return -1; M_INV_X(feature_point_res) = FLOAT_MAT_ELEM(&pix, 0, 0); M_INV_Y(feature_point_res) = FLOAT_MAT_ELEM(&pix, 1, 0); return 1; }