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;
}