Esempio n. 1
0
        /// <summary>
        /// returns the position and orientation of the camera
        /// </summary>
        /// <param name="position"></param>
        /// <param name="orientation"></param>
        public void GetCameraPositionOrientation(ref Vector3D position, ref Quaternion orientation)
        {
            /*
            Quaternion qRO = new Quaternion(0.0f, 1.0f, 0.0f, 0.0f);
            ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model();

            threed_motion_model.func_xp(scene.get_xv());
            threed_motion_model.func_r(threed_motion_model.get_xpRES());
            threed_motion_model.func_q(threed_motion_model.get_xpRES());

            orientation = threed_motion_model.get_qRES(); //.Multiply(qRO);
            position = threed_motion_model.get_rRES();
            */

            Vector state = scene.get_xv();
            position = new Vector3D(state[0], state[1], state[2]);
            //orientation = new Quaternion(state[4], state[5], state[6], state[3]);
            orientation = new Quaternion(state[3], state[4], state[5], state[6]); //.Multiply(qRO);
            
        }
Esempio n. 2
0
        public static MatrixFixed dq3_by_dq2(Quaternion q1)
        {
            float[] a = new float[16];
            
            a[0] = q1.R();
            a[1] = -q1.X();
            a[2] = -q1.Y();
            a[3] = -q1.Z();

            a[4] = q1.X();
            a[5] = q1.R();
            a[6] = q1.Z();
            a[7] = -q1.Y();

            a[8] = q1.Y();
            a[9] = -q1.Z();
            a[10] = q1.R();
            a[11] = q1.X();

            a[12] = q1.Z();
            a[13] = q1.Y();
            a[14] = -q1.X();
            a[15] = q1.R();

            MatrixFixed M = new MatrixFixed(a);
            return M;
        }
Esempio n. 3
0
 // Combine two rotations (this is VERY costly).
 // @param aa the inner rotation
 // @returns the resulting rotation (like this*q for matrices)
 public AngleAxis Multiply (AngleAxis aa)
 {
     // This is best done as quaternions
     Quaternion q1 = new Quaternion(this);
     Quaternion q2 = new Quaternion(aa);
     return new AngleAxis(q1.Multiply(q2));
 }
Esempio n. 4
0
        /// <summary>
        /// Create a state vector x_v from its component parts. Puts the matrices r, q, v, omega into their right places.
        /// </summary>
        /// <param name="r"></param>
        /// <param name="q"></param>
        /// <param name="v"></param>
        /// <param name="omega"></param>
        /// <param name="xv"></param>
        public void compose_xv(ref Vector3D r, ref Quaternion q, ref Vector3D v, ref Vector3D omega, ref Vector xv)
        {  
            xv.Update(r.GetVNL3(), 0);

            xv.Update(q.GetRXYZ(), 3);

            xv.Update(v.GetVNL3(), 7);
   
            xv.Update(omega.GetVNL3(), 10);
        }
Esempio n. 5
0
        public static MatrixFixed dqnorm_by_dq(Quaternion q)
        {
            float qq = q.GetR()*q.GetR() + 
                        q.GetX()*q.GetX() + 
                        q.GetY()*q.GetY() + 
                        q.GetZ()*q.GetZ();


            float[] a = new float[16];
            
            a[0] = dqi_by_dqi(q.GetR(), qq);
            a[1] = dqi_by_dqj(q.GetR(), q.GetX(), qq);
            a[2] = dqi_by_dqj(q.GetR(), q.GetY(), qq);
            a[3] = dqi_by_dqj(q.GetR(), q.GetZ(), qq);

            a[4] = dqi_by_dqj(q.GetX(), q.GetR(), qq);
            a[5] = dqi_by_dqi(q.GetX(), qq);
            a[6] = dqi_by_dqj(q.GetX(), q.GetY(), qq);
            a[7] = dqi_by_dqj(q.GetX(), q.GetZ(), qq);

            a[8] = dqi_by_dqj(q.GetY(), q.GetR(), qq);
            a[9] = dqi_by_dqj(q.GetY(), q.GetX(), qq);
            a[10] = dqi_by_dqi(q.GetY(), qq);
            a[11] = dqi_by_dqj(q.GetY(), q.GetZ(), qq);

            a[12] = dqi_by_dqj(q.GetZ(), q.GetR(), qq);
            a[13] = dqi_by_dqj(q.GetZ(), q.GetX(), qq);
            a[14] = dqi_by_dqj(q.GetZ(), q.GetY(), qq);
            a[15] = dqi_by_dqi(q.GetZ(), qq);

            MatrixFixed M = new MatrixFixed(a);
            return M;		
        }
Esempio n. 6
0
 public bool EqualTo(Quaternion q)
 {
     return ((q._x == _x) && (q._y == _y) && (q._z == _z) && (q._r == _r));
 }
Esempio n. 7
0
 /*
 public RotationMatrix(AngleAxis angleaxis) : base(3,3)
 {
     Set(angleaxis);
 }
 */
 public RotationMatrix(Quaternion q) : base(3,3)
 {
     Set(q);
 }
Esempio n. 8
0
        public static MatrixFixed dR_by_dqz(Quaternion q)
        {
            float q0 = q.R();
            float qx = q.X();
            float qy = q.Y();
            float qz = q.Z();

            float[] a = new float[9];
            a[0] = -2*qz;
            a[1] = -2*q0;
            a[2] = 2*qx;

            a[3] = 2*q0;
            a[4] = -2*qz;
            a[5] = 2*qy;

            a[6] = 2*qx;
            a[7] = 2*qy;
            a[8] = 2*qz;

            MatrixFixed M = new MatrixFixed(3,3);
            return M;
        }
Esempio n. 9
0
 /// <summary>
 /// returns the current camera position and orientation
 /// </summary>
 /// <param name="position"></param>
 /// <param name="orientation"></param>
 public void getCameraPositionOrientation(ref Vector3D position, ref Quaternion orientation)
 {
     monoslaminterface.GetCameraPositionOrientation(ref position, ref orientation);
 }
Esempio n. 10
0
        public override void func_xpose_and_Rpose(Vector xv)
        {
            func_xp(xv);

            // Turn xp vector (x, y, z, q0, q1, q2, q3) into pose
            xposeRES = xpRES.Extract(3, 0);

            Vector qvec = xpRES.Extract(4, 3);
            Quaternion q = new Quaternion();
            q.SetRXYZ(qvec);

            RposeRES = q.RotationMatrix();
        }
Esempio n. 11
0
        public void SetUp3DDisplays()
        {
            // Set display virtual camera parameters to match those of the real camera
            // being used in MonoSLAM
            Partially_Initialised_Feature_Measurement_Model default_pifmm =
                monoslaminterface.GetDefaultFeatureTypeForInitialisation();

            if (default_pifmm != null)
            {

                Line_Init_Wide_Point_Feature_Measurement_Model default_wide_pifmm =
                    (Line_Init_Wide_Point_Feature_Measurement_Model)default_pifmm;

                Graphics_Fku = default_wide_pifmm.get_camera().Fku();
                Graphics_Fkv = default_wide_pifmm.get_camera().Fkv();
                Graphics_U0 = default_wide_pifmm.get_camera().U0();
                Graphics_V0 = default_wide_pifmm.get_camera().V0();
                Graphics_Kd1 = default_wide_pifmm.get_camera().Kd1();

                // First display for external 3D view
                /*
                threedtool = new ThreeDToolGlowWidget(this,
                     controlpanel1->Width() + controlpanel2->Width(), 0,
                         monoslaminterface->GetCameraWidth(),
                         monoslaminterface->GetCameraHeight());
                threedtool->DrawEvent.Attach(this, &MonoSLAMGlow::Draw3D);
                threedtool->ProcessHitsEvent.Attach(this, &MonoSLAMGlow::ProcessHits);
                threedtool->SetCameraParameters(Graphics_Fku, Graphics_Fkv, 
                      Graphics_U0, Graphics_V0);
                 */


                // Set start position for GL camera in 3D tool
                // This is x, y, z position
                Vector rthreed = new Vector(0.0f, 0.2f, -1.5f);
                // This is camera orientation in my normal coordinate system
                // (z forward, y up, x left)
                Quaternion qWRthreed = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                // We need to adjust by this rotation to fit GL coordinate frame
                // (z backward, y up, x right)
                // So rotate by pi about y axis
                Quaternion qROthreed = new Quaternion(0.0f, 1.0f, 0.0f, 0.0f);
                Quaternion qWOthreed = qWRthreed.Multiply(qROthreed);
                //threedtool.SetCameraPositionOrientation(rthreed, qWOthreed);

                // Second 3D display for images and augmented reality
                /*
                image_threedtool = new ThreeDToolGlowWidget(this,
                                           controlpanel1->Width() + controlpanel2->Width(), threedtool->Height(),
                                           monoslaminterface->GetCameraWidth(), monoslaminterface->GetCameraHeight());
                image_threedtool.DrawEvent.Attach(this, &MonoSLAMGlow::ImageDraw3D);
                image_threedtool.ProcessHitsEvent.Attach(this, &MonoSLAMGlow::ImageProcessHits);
                image_threedtool.SetCameraParameters(Graphics_Fku, Graphics_Fkv, Graphics_U0, Graphics_V0);
                */

                // Set up initial state of virtual camera for image display to match
                // state vector
                Scene_Single scene = monoslaminterface.GetScene();
                if (scene != null)
                {
                    Vector v = scene.get_xv();
                    if (v != null)
                    {
                        scene.get_motion_model().func_xp(v);
                        ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model();
                        Vector3D r = threed_motion_model.get_rRES();
                        Quaternion qWR = threed_motion_model.get_qRES();
                        // q is qWR between world frame and Scene robot frame
                        // We need to adjust by this rotation to fit GL coordinate frame
                        // (z backward, y up, x right)
                        // So rotate by pi about y axis
                        Quaternion qRO = new Quaternion(0.0f, 1.0f, 0.0f, 0.0f);
                        Quaternion qWO = qWR.Multiply(qRO);
                        //image_threedtool.SetCameraPositionOrientation(r, qWO);
                    }
                    else
                    {
                        Debug.WriteLine("Scene xp not defined");
                    }
                }
                else
                {
                    Debug.WriteLine("No scene object defined");
                }
            }
            else
            {
                Debug.WriteLine("No partially initialised feature measurement model found");
            }
        }
Esempio n. 12
0
        /// <summary>
        /// Form the covariance matrix  Q  of the process noise associated with  x_v .
        /// </summary>
        /// <param name="xv"></param>
        /// <param name="v"></param>
        /// <param name="delta_t"></param>
        public override void func_Q(Vector xv, Vector v, float delta_t)
        {
            // Fill noise covariance matrix Pnn: this is the covariance of 
            // the noise vector (V)
            //                  (Omega)
            // that gets added to the state. 
            // Form of this could change later, but for now assume that 
            // V and Omega are independent, and that each of their components is
            // independent... 
            float linear_velocity_noise_variance =
                       SD_A_component_filter * SD_A_component_filter * delta_t * delta_t;
            float angular_velocity_noise_variance =
                       SD_alpha_component_filter * SD_alpha_component_filter * delta_t * delta_t;

            // Independence means that the matrix is diagonal
            MatrixFixed Pnn = new MatrixFixed(6, 6);
            Pnn.Fill(0.0f);
            Pnn.Put(0, 0, linear_velocity_noise_variance);
            Pnn.Put(1, 1, linear_velocity_noise_variance);
            Pnn.Put(2, 2, linear_velocity_noise_variance);
            Pnn.Put(3, 3, angular_velocity_noise_variance);
            Pnn.Put(4, 4, angular_velocity_noise_variance);
            Pnn.Put(5, 5, angular_velocity_noise_variance);

            // Form Jacobian dxnew_by_dn
            // Is like this:
            // I * delta_t     0
            // 0               dqnew_by_dOmega

            // Start by zeroing
            MatrixFixed dxnew_by_dn = new MatrixFixed(7, 6);
            dxnew_by_dn.Fill(0.0f);

            // The translation part is just I \Delta t
            MatrixFixed Temp33A = new MatrixFixed(3, 3);
            Temp33A.SetIdentity();
            Temp33A *= delta_t;
            dxnew_by_dn.Update(Temp33A, 0, 0);

            // qnew = q x \Omega \Deltat
            // dqnew_by_d\Omega = dqnew_by_d\Omega\Delta t . d\Omega\Delta t_by_d\Omega

            // Get the first part
            Vector qRXYZ = xv.Extract(4, 3);
            Quaternion qold = new Quaternion();
            qold.SetRXYZ(qRXYZ);
            MatrixFixed Temp44A = MatrixFixed.dq3_by_dq1(qold);

            // Use function below for dqwt_by_dOmega
            Vector Omega = new Vector(3);
            Omega.Fill(SD_alpha_component_filter);

            MatrixFixed Temp43A = new MatrixFixed(4, 3);
            dqomegadt_by_domega(new Vector3D(Omega), delta_t, Temp43A);
            // Multiply them together
            MatrixFixed Temp43B = Temp44A * Temp43A;
            // And then plug into Jacobian
            dxnew_by_dn.Update(Temp43B, 3, 3);

            // Finally do Q = dxnew_by_dn . Pnn . dxnew_by_dnT
            QxRES.Update(dxnew_by_dn * Pnn * dxnew_by_dn.Transpose());

        }
Esempio n. 13
0
 public AngleAxis(Quaternion q)
 {
     Set(q);
 }
Esempio n. 14
0
 public static MatrixFixed dRq_times_a_by_dq(Quaternion q, Vector a)
 {
     Vector3D v = new Vector3D(a);
     return (dRq_times_a_by_dq(q, v));
 }
Esempio n. 15
0
 public void Set(Quaternion q)
 {
     this.Equals(q.RotationMatrix());
 }
Esempio n. 16
0
        //returns matrix of dimensions (3,4)
        public static MatrixFixed dRq_times_a_by_dq (Quaternion q, Vector3D a)
        {
            MatrixFixed aMat = new MatrixFixed(3,1);
            aMat[0, 0] = a.GetX(); 
            aMat[1, 0] = a.GetY(); 
            aMat[2, 0] = a.GetZ(); 

            MatrixFixed TempR = new MatrixFixed(3,3);
            MatrixFixed Temp31 = new MatrixFixed(3,1);
            MatrixFixed dRq_times_a_by_dq = new MatrixFixed(3,4);

            // Make Jacobian by stacking four vectors together side by side
            TempR = dR_by_dq0(q);
            Temp31 = TempR * aMat;
            dRq_times_a_by_dq.Update(Temp31, 0, 0);

            TempR = dR_by_dqx(q);
            Temp31 = TempR * aMat;
            dRq_times_a_by_dq.Update(Temp31, 0, 1);

            TempR = dR_by_dqy(q);
            Temp31 = TempR * aMat;
            dRq_times_a_by_dq.Update(Temp31, 0, 2);

            TempR = dR_by_dqz(q);
            Temp31 = TempR * aMat;
            dRq_times_a_by_dq.Update(Temp31, 0, 3);

            return dRq_times_a_by_dq;
        }
Esempio n. 17
0
        /// <summary>
        /// 
        /// </summary>
        /// <param name="xv">position and orientation of the camera</param>
        /// <param name="u">Control vector of accelerations</param>
        /// <param name="delta_t">time step in seconds</param>
        public override void func_fv_and_dfv_by_dxv(Vector xv, Vector u, float delta_t)
        {
            Vector3D rold, vold, omegaold, rnew, vnew, omeganew;
            Quaternion qold, qnew;            

            // Separate things out to make it clearer
            rold = new Vector3D(0, 0, 0);
            vold = new Vector3D(0, 0, 0);
            omegaold = new Vector3D(0, 0, 0);
            qold = new Quaternion();
            extract_r_q_v_omega(xv, rold, qold, vold, omegaold);

            Vector3D acceleration = new Vector3D(u);
           
            // rnew = r + v * delta_t
            //rnew = (Vector3D)((Point3D)rold + (Point3D)vold * delta_t);
            rnew = new Vector3D(rold + vold * delta_t);

            // qnew = q x q(omega * delta_t)

            // Keep qwt ( = q(omega * delta_t)) for later use
            Quaternion qwt = new Quaternion(omegaold * delta_t);

            qnew = qold.Multiply(qwt);

            // vnew = v
            vnew = new Vector3D(vold + acceleration * delta_t);
  
            // omeganew = omega
            omeganew = omegaold;

            

            // Put it all together
            compose_xv(ref rnew, ref qnew, ref vnew, ref omeganew, ref fvRES);

            // cout << "rold qold vold omegaold" << rold << qold 
            //      << vold << omegaold;
            // cout << "rnew qnew vnew omeganew" << rnew << qnew 
            //      << vnew << omeganew;

            // Now on to the Jacobian...
            // Identity is a good place to start since overall structure is like this
            // I       0             I * delta_t   0
            // 0       dqnew_by_dq   0             dqnew_by_domega
            // 0       0             I             0
            // 0       0             0             I
            dfv_by_dxvRES.SetIdentity();

            // Fill in dxnew_by_dv = I * delta_t
            MatrixFixed Temp33A = new MatrixFixed(3,3);
            Temp33A.SetIdentity();
            Temp33A *= delta_t;
            dfv_by_dxvRES.Update(Temp33A, 0, 7);

            // Fill in dqnew_by_dq
            // qnew = qold x qwt  ( = q3 = q2 x q1 in Scene/newbits.cc language)
            MatrixFixed Temp44A = MatrixFixed.dq3_by_dq2(qwt); //4,4
            dfv_by_dxvRES.Update(Temp44A, 3, 3);

            // Fill in dqnew_by_domega = d(q x qwt)_by_dqwt . dqwt_by_domega
            Temp44A = MatrixFixed.dq3_by_dq1(qold); // Temp44A is d(q x qwt) by dqwt
 
            // Use function below for dqwt_by_domega
            MatrixFixed Temp43A = new MatrixFixed(4,3);
            dqomegadt_by_domega(omegaold, delta_t, Temp43A);
            // Multiply them together
            MatrixFixed Temp43B = Temp44A * Temp43A;
            // And plug it in
            dfv_by_dxvRES.Update(Temp43B, 3, 10);
             
             

            // cout << "dfv_by_dxvRES" << dfv_by_dxvRES;
        }
Esempio n. 18
0
        public Quaternion normalise_quaternion(Quaternion q)
        {
            // qq is current magnitude of q
            float qq = (float)Math.Sqrt(q.GetR() * q.GetR() + 
		                     q.GetX()*q.GetX() + q.GetY()*q.GetY() + q.GetZ()*q.GetZ());

            Quaternion qnorm = new Quaternion(
                q.GetX() / qq,
		        q.GetY() / qq,
		        q.GetZ() / qq,
		        q.GetR() / qq);
            return qnorm;
        }
Esempio n. 19
0
        /// <summary>
        /// Fill noise covariance matrix Pnn: this is the covariance of 
        /// the noise vector (V)
        ///                  (Omega)
        /// that gets added to the state. 
        /// Form of this could change later, but for now assume that 
        /// V and Omega are independent, and that each of their components is
        /// independent... 
        /// </summary>
        /// <param name="xv"></param>
        /// <param name="v"></param>
        /// <param name="delta_t"></param>
        public override void func_Q(Vector xv, Vector v, float delta_t)
        {
            float linear_velocity_noise_variance = 
                SD_A_component_filter * SD_A_component_filter * delta_t * delta_t;
            float angular_velocity_noise_variance =
                SD_alpha_component_filter * SD_alpha_component_filter * delta_t * delta_t;

            // Independence means that the matrix is diagonal
            MatrixFixed Pnn = new MatrixFixed(6,6);
            Pnn.Fill(0.0f);
            Pnn.Put(0, 0, linear_velocity_noise_variance);
            Pnn.Put(1, 1, linear_velocity_noise_variance);
            Pnn.Put(2, 2, linear_velocity_noise_variance);
            Pnn.Put(3, 3, angular_velocity_noise_variance);
            Pnn.Put(4, 4, angular_velocity_noise_variance);
            Pnn.Put(5, 5, angular_velocity_noise_variance);

            // Form Jacobian dxnew_by_dn
            // Is like this:
            // I * delta_t     0
            // 0               dqnew_by_dOmega
            // I               0
            // 0               I

            // Start by zeroing
            MatrixFixed dxnew_by_dn = new MatrixFixed(13,6);
            dxnew_by_dn.Fill(0.0f);

            // Fill in easy bits first
            MatrixFixed Temp33A = new MatrixFixed(3,3);
            Temp33A.SetIdentity();
  
            dxnew_by_dn.Update(Temp33A, 7, 0);
            dxnew_by_dn.Update(Temp33A, 10, 3);
            Temp33A *= delta_t;
            dxnew_by_dn.Update(Temp33A, 0, 0);

            // Tricky bit is dqnew_by_dOmega
            // Is actually the same calculation as in func_fv...
            // Since omega and Omega are additive...?
            Vector3D rold = new Vector3D(0, 0, 0);
            Vector3D vold = new Vector3D(0, 0, 0);
            Vector3D omegaold = new Vector3D(0, 0, 0);
            Quaternion qold=new Quaternion();
            extract_r_q_v_omega(xv, rold, qold, vold, omegaold); // overkill but easy
            // Fill in dqnew_by_domega = d(q x qwt)_by_dqwt . dqwt_by_domega
            // Temp44A is d(q x qwt) by dqwt
            MatrixFixed Temp44A = MatrixFixed.dq3_by_dq1(qold); 
            // Use function below for dqwt_by_domega
            MatrixFixed Temp43A = new MatrixFixed(4,3);
            dqomegadt_by_domega(omegaold, delta_t, Temp43A);
            // Multiply them together
            MatrixFixed Temp43B = Temp44A * Temp43A;
            // And then plug into Jacobian
            dxnew_by_dn.Update(Temp43B, 3, 3);

            // Finally do Q = dxnew_by_dn . Pnn . dxnew_by_dnT
            QxRES.Update( dxnew_by_dn * Pnn * dxnew_by_dn.Transpose() );

            //  cout << "QxRES" << QxRES;
        }
Esempio n. 20
0
        // Combine two rotations.
        // @param q the inner rotation
        // @returns the resulting rotation (like this*q for matrices)
        public Quaternion Multiply (Quaternion q)
        {
            // real and img parts of args
            float r1 = this._r;                  
            float r2 = q._r;
            Vector3D i1 = this.Imaginary();
            Vector3D i2 = q.Imaginary();

            // Real is product of real, and dot-product of imag
            float real_v = (r1 * r2) - (i1 * i2);

            // Imag is cross product of imag + real*imag for each
            Point3D img = Point3D.VectorProduct(i1, i2);
            //Point3D img2 = (img + (i2 * r1) + (i1 * r2));

            // Finally, build the result
            Quaternion prod = new Quaternion(img.GetX(), img.GetY(), img.GetZ(), real_v);
            return prod;
        }
Esempio n. 21
0
        /// <summary>
        /// Extract the component parts of the state  x_v . Fills matrices r, q, v, omega with values.
        /// </summary>
        /// <param name="xv"></param>
        /// <param name="r"></param>
        /// <param name="q"></param>
        /// <param name="v"></param>
        /// <param name="omega"></param>
        public void extract_r_q_v_omega(Vector xv, Vector3D r, Quaternion q, Vector3D v, Vector3D omega)
        {
            r.SetVNL3(xv.Extract(3, 0));

            Vector qRXYZ = xv.Extract(4, 3);
            q.SetRXYZ(qRXYZ);

            v.SetVNL3(xv.Extract(3, 7));

            omega.SetVNL3(xv.Extract(3, 10));
        }
Esempio n. 22
0
 public bool NotEqualTo (Quaternion q)
 {
     return ((q._x != _x) || (q._y != _y) || (q._z != _z) || (q._r != _r));
 }
Esempio n. 23
0
 public void Set(Quaternion q)
 {
     float r = q.GetR();
     _x = q.GetX();
     _y = q.GetY();
     _z = q.GetZ();
     float norm = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);
     if (norm > 0) 
     {
         float scaling = 2.0f * (float)Math.Atan2(norm, r) / norm;
         _x *= scaling;
         _y *= scaling;
         _z *= scaling;
     }
 }