// 
// 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;
}
예제 #2
0
//----------------------------------------------------------------------------
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;
}