示例#1
0
        // @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);
        }
示例#2
0
        /// <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];
        }
示例#3
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
        }
示例#4
0
        public void Set(Vector3D angleaxis)
        {
            float angle = angleaxis.Norm();

            Set(angleaxis, angle);
        }
示例#5
0
        /// <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];
        }
示例#6
0
 public void Set(Vector3D angleaxis)
 {
     float angle = angleaxis.Norm();
     Set(angleaxis, angle);
 }