//
// 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;
}
//----------------------------------------------------------------------------
BspNode* BspNodes::CreateNode (const Vector2f& v0, const Vector2f& v1,
VertexColor3Effect* effect, const Float3& color)
{
// Create the model-space separating plane.
Vector2f dir = v1 - v0;
AVector normal(dir[1], -dir[0], 0.0f);
normal.Normalize();
float constant = normal[0]*v0[0] + normal[1]*v0[1];
HPlane modelPlane(normal, constant);
// Create the BSP node.
BspNode* bsp = new0 BspNode(modelPlane);
VertexFormat* vformat = VertexFormat::Create(2,
VertexFormat::AU_POSITION, VertexFormat::AT_FLOAT3, 0,
VertexFormat::AU_COLOR, VertexFormat::AT_FLOAT3, 0);
// Create the rectangle representation of the model plane and set the
// vertex colors to the specified color.
float xExtent = 0.5f*dir.Length();
float yExtent = 0.125f;
TriMesh* rect = StandardMesh(vformat).Rectangle(2, 2, xExtent, yExtent);
VertexBufferAccessor vba(rect);
for (int i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, i) = color;
}
rect->SetEffectInstance(effect->CreateInstance());
// Set the position and orientation for the world-space plane.
APoint trn(0.5f*(v0[0] + v1[0]), 0.5f*(v0[1] + v1[1]), yExtent + 0.001f);
HMatrix zRotate(AVector::UNIT_Z, Mathf::ATan2(dir.Y(),dir.X()));
HMatrix xRotate(AVector::UNIT_X, Mathf::HALF_PI);
HMatrix rotate = zRotate*xRotate;
rect->LocalTransform.SetTranslate(trn);
rect->LocalTransform.SetRotate(rotate);
bsp->AttachCoplanarChild(rect);
return bsp;
}