/* constructor
*/
Intrinsic::Intrinsic (int w, int h, double focal, double c1, double c2, double k1, double k2, double k3, double k4, double k5, double w_mm, double h_mm)
{
f = focal;
width = w;
height = h;
cc = Vec2d(c1,c2);
CC = denormalize(cc);
kc[0] = k1; kc[1] = k2; kc[2] = k3; kc[3] = k4; kc[4] = k5;
pixel_width_mm = w_mm;
pixel_height_mm = h_mm;
double fov = toRadians(70.0); // estimated Ladybug FOV, given the fact that 5 cameras cover 360 degrees + overlap
pixel_angle = (double)fov / height; // radians per pixel
dist_model = POLYNOMIAL_DISTORTION;
value = 0.0;
Vec2d FF = denormalize(Vec2d(f,f));
focal_x = FF[0];
focal_y = FF[1];
Vec2d a = Vec2d( -0.5, -(double)h/(2.0*w) );
Vec2d b = Vec2d( 0.5, -(double)h/(2.0*w) );
Vec2d c = Vec2d( 0.5, (double)h/(2.0*w) );
Vec2d d = Vec2d( -0.5, (double)h/(2.0*w) );
LEFT = Edge2D(a, d, 0);
RIGHT = Edge2D(b, c, 0);
TOP = Edge2D(c, d, 0);
BOTTOM = Edge2D(a, b, 0);
}
/* constructor
*/
Intrinsic::Intrinsic (int id, int w, int h, double focal, double c1, double c2, double v, double w_mm, double h_mm)
{
_id = id;
width = w;
height = h;
f = focal;
cc = Vec2d(c1,c2);
CC = denormalize(cc);
value = v;
dist_model = SPHERICAL_DISTORTION;
kc[0] = 0.0; kc[1] = 0.0; kc[2] = 0.0; kc[3] = 0.0; kc[4] = 0.0;
Vec2d FF = denormalize(Vec2d(f,f));
focal_x = 345.36; //5*FF[0];
focal_y = 345.36; //FF[1];
pixel_width_mm = w_mm;
pixel_height_mm = h_mm;
double fov = toRadians(70.0); // estimated Ladybug FOV, given the fact that 5 cameras cover 360 degrees + overlap
pixel_angle = (double)fov / width; // radians per pixel
Vec2d a = Vec2d( -0.5, -(double)h/(2.0*w) );
Vec2d b = Vec2d( 0.5, -(double)h/(2.0*w) );
Vec2d c = Vec2d( 0.5, (double)h/(2.0*w) );
Vec2d d = Vec2d( -0.5, (double)h/(2.0*w) );
LEFT = Edge2D(a, d, 0);
RIGHT = Edge2D(b, c, 0);
TOP = Edge2D(c, d, 0);
BOTTOM = Edge2D(a, b, 0);
}
/* Rectify a 2D image edge */
Edge2D Camera::rectifyEdge (Edge2D edge)
{
Vec2d ar,br;
ar = rectifyPixel(edge._a);
br = rectifyPixel(edge._b);
return Edge2D (ar,br,edge._id);
}
bool Polygon::Contains2D(const float2 &localSpacePoint) const
{
if (p.size() < 3)
return false;
LineSegment l(float3(localSpacePoint, 0), float3(localSpacePoint,0) + float3(1,1,0).Normalized());
int numIntersections = 0;
for(size_t i = 0; i < p.size(); ++i)
if (Edge2D(i).Intersects(l))
++numIntersections;
return numIntersections % 2 == 1;
}
bool Polygon::Contains2D(const LineSegment &localSpaceLineSegment) const
{
if (p.size() < 3)
return false;
if (!Contains2D(localSpaceLineSegment.a.xy()) || !Contains2D(localSpaceLineSegment.b.xy()))
return false;
for(size_t i = 0; i < p.size(); ++i)
if (Edge2D(i).Intersects(localSpaceLineSegment))
return false;
return true;
}
bool Polygon::Contains2D(const LineSegment &localSpaceLineSegment) const
{
if (p.size() < 3)
return false;
///\todo Reimplement this!
// if (!Contains2D(localSpaceLineSegment.a.xy()) || !Contains2D(localSpaceLineSegment.b.xy()))
// return false;
for(int i = 0; i < (int)p.size(); ++i)
if (Edge2D(i).Intersects(localSpaceLineSegment))
return false;
return true;
}
/* Debugging only. Check the sanity of rectify and distortion methods
*/
void Intrinsic::distorsionChecker()
{
// print
print();
// check clipping algorithm
Edge2D edge = Edge2D(Vec2d(-200,-100),Vec2d(10,20));
Edge2D edge_clipped = clip(edge);
LOG(LEVEL_DEBUG,"(%f,%f) => (%f,%f)",edge._a[0],edge._a[1],edge_clipped._a[0],edge_clipped._a[1]);
LOG(LEVEL_DEBUG,"(%f,%f) => (%f,%f)",edge._b[0],edge._b[1],edge_clipped._b[0],edge_clipped._b[1]);
//pick a bunch of 2D points and check their rectified value
Vec2d point = Vec2d(2.500000000000000e+001 , 6.950000000000000e+001);
Vec2d point_rect = rectify(normalize(point));
LOG(LEVEL_DEBUG,"(%f,%f) ==> (%f,%f) should be (%f,%f)", point[0],point[1],point_rect[0],point_rect[1],
-1.271644101537981e+000 , -6.149558565018068e-001);
point = Vec2d(2.560000000000000e+002 , 1.950000000000000e+001);
point_rect = rectify(normalize(point));
LOG(LEVEL_DEBUG,"(%f,%f) ==> (%f,%f) should be (%f,%f)", point[0],point[1],point_rect[0],point_rect[1],
-1.559022165084900e-002 , -5.935958630008766e-001);
point = Vec2d(2.560000000000000e+002 , 6.650000000000000e+001);
point_rect = rectify(normalize(point));
LOG(LEVEL_DEBUG,"(%f,%f) ==> (%f,%f) should be (%f,%f)", point[0],point[1],point_rect[0],point_rect[1],
-1.308190696800208e-002 , -3.973240271620430e-001);
//pick a bunch of points
// perform successive distortion and rectification and check the discrepancy
int n_points = 10;
for (int i=0;i<n_points;i++) {
Vec2d point = Vec2d ((double)rand()/RAND_MAX,(double)rand()/RAND_MAX);
Vec2d point_dist = distort(point);
Vec2d point_dist_2 = distort(rectify(point_dist));
LOG(LEVEL_DEBUG,"(%f,%f) => (%f,%f) d = %f\n",point_dist[0],point_dist[1],point_dist_2[0],point_dist_2[1],len(point_dist-point_dist_2));
}
}
/* Distort a 2D image edge
*/
Edge2D Intrinsic::distort ( Edge2D &edge )
{
return Edge2D( distort( edge._a), distort( edge._b ) );
}
/* Rectify a 2D image edge
*/
Edge2D Intrinsic::rectify ( Edge2D &edge )
{
return Edge2D( rectify( edge._a), rectify( edge._b ) );
}
