// @returns the axis of rotation. */
public Vector3D Axis()
{
Vector3D _i = Imaginary();
float l = _i.Norm();
if (l < 1e-8)
{
_i.Set(0.0f, 0.0f, 0.0f);
}
else
{
Point3D _i2 = (Point3D)_i / l;
_i = new Vector3D(_i2);
}
return(_i);
}
/// <summary>
/// Note that the axis doesn't have to be a unit vector.
/// </summary>
/// <param name="axis">the axis around which to rotate</param>
/// <param name="angle">the angle of rotation in radians</param>
public void Set(Vector3D axis, float angle)
{
// start with an identity matrix.
this.SetIdentity();
// Identity is all that can be deduced from zero angle
if (angle == 0)
{
return;
}
// normalize to a unit vector u
float[] u = new float[3];
float norm;
norm = 1.0f / axis.Norm();
u[0] = axis.GetX() * norm;
u[1] = axis.GetY() * norm;
u[2] = axis.GetZ() * norm;
// add (cos(angle)-1)*(1 - u u').
float cos_angle = (float)Math.Cos(angle);
for (uint i = 0; i < 3; ++i)
{
for (uint j = 0; j < 3; ++j)
{
this[(int)i, (int)j] += (cos_angle - 1) * ((i == j ? 1:0) - u[i] * u[j]);
}
}
// add sin(angle) * [u]
float sin_angle = (float)Math.Sin(angle);
this[0, 1] -= sin_angle * u[2];
this[0, 2] += sin_angle * u[1];
this[1, 0] += sin_angle * u[2];
this[1, 2] -= sin_angle * u[0];
this[2, 0] -= sin_angle * u[1];
this[2, 1] += sin_angle * u[0];
}
/// <summary>
///
/// </summary>
/// <param name="xp">current robot position state</param>
/// <param name="yi">3D position of the feature</param>
/// <param name="xp_orig">robot position state when the feature was initially observed</param>
/// <param name="hi">image coordinates of the feature</param>
/// <returns></returns>
public override uint visibility_test(Vector xp, Vector yi, Vector xp_orig, Vector hi)
{
float image_search_boundary = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).IMAGE_SEARCH_BOUNDARY;
uint wdth = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.ImageWidth();
uint hght = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.ImageHeight();
uint cant_see_flag = 0;
// Test image boundaries
float x = hi[0];
float y = hi[1];
if ((x < 0.0f + image_search_boundary) ||
(x > (float)(wdth - 1 - image_search_boundary)))
{
cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.LEFT_RIGHT_FAIL;
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Visibility test left / right fail.");
}
if ((y < 0.0f + image_search_boundary) ||
(y > (float)(hght - 1 - image_search_boundary)))
{
cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.UP_DOWN_FAIL;
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Visibility test up / down fail.");
}
// Do tests on length and angle of predicted view
// hLWi is current predicted vector from head to feature in
// world frame
// This function gives relative position of feature
func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, xp);
// Test the feature's not behind the camera (because projection
// may do strange things)
if (zeroedyiRES[2] <= 0)
{
cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.BEHIND_CAMERA_FAIL;
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Behind camera fail.");
}
((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.func_q(xp);
RotationMatrix RWR = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_qRES().RotationMatrix();
Vector3D hLWi = new Vector3D(RWR * (new Point3D(zeroedyiRES)));
// hLWi_orig is vector from head to feature in world frame
// WHEN THAT FEATURE WAS FIRST MEASURED: i.e. when the image
// patch was saved
func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, xp_orig);
((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.func_q(xp_orig);
RotationMatrix RWR_orig = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_qRES().RotationMatrix();
Vector3D hLWi_orig = new Vector3D(RWR_orig * (new Point3D(zeroedyiRES)));
// Compare hLWi and hLWi_orig for length and orientation
float mod_hLWi = hLWi.Norm();
float mod_hLWi_orig = hLWi_orig.Norm();
float length_ratio = mod_hLWi / mod_hLWi_orig;
float max_length_ratio = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).MAXIMUM_LENGTH_RATIO;
if ((length_ratio > max_length_ratio) ||
(length_ratio < (1.0f / max_length_ratio)))
{
cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.DISTANCE_FAIL;
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Distance fail.");
}
float dot_prod = hLWi * hLWi_orig;
float angle = (float)Math.Acos(dot_prod / (mod_hLWi * mod_hLWi_orig));
if (angle == float.NaN) Debug.WriteLine("Maths error: " + dot_prod + " / " + (mod_hLWi * mod_hLWi_orig));
angle = (angle >= 0.0f ? angle : -angle); // Make angle positive
if (angle > ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).MAXIMUM_ANGLE_DIFFERENCE)
{
cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.ANGLE_FAIL;
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Angle fail.");
}
return cant_see_flag; // 0 if OK, otherwise error code
}
public void Set(Vector3D angleaxis)
{
float angle = angleaxis.Norm();
Set(angleaxis, angle);
}
/// <summary>
/// Note that the axis doesn't have to be a unit vector.
/// </summary>
/// <param name="axis">the axis around which to rotate</param>
/// <param name="angle">the angle of rotation in radians</param>
public void Set(Vector3D axis, float angle)
{
// start with an identity matrix.
this.SetIdentity();
// Identity is all that can be deduced from zero angle
if (angle == 0)
return;
// normalize to a unit vector u
float[] u = new float[3];
float norm;
norm = 1.0f / axis.Norm();
u[0]=axis.GetX()*norm;
u[1]=axis.GetY()*norm;
u[2]=axis.GetZ()*norm;
// add (cos(angle)-1)*(1 - u u').
float cos_angle = (float)Math.Cos(angle);
for (uint i=0; i<3; ++i)
for (uint j=0; j<3; ++j)
this[(int)i,(int)j] += (cos_angle-1) * ((i==j ? 1:0) - u[i]*u[j]);
// add sin(angle) * [u]
float sin_angle = (float)Math.Sin(angle);
this[0,1] -= sin_angle*u[2];
this[0,2] += sin_angle*u[1];
this[1,0] += sin_angle*u[2];
this[1,2] -= sin_angle*u[0];
this[2,0] -= sin_angle*u[1];
this[2,1] += sin_angle*u[0];
}
public void Set(Vector3D angleaxis)
{
float angle = angleaxis.Norm();
Set(angleaxis, angle);
}