/// <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); }
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; }
// 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)); }
/// <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); }
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; }
public bool EqualTo(Quaternion q) { return ((q._x == _x) && (q._y == _y) && (q._z == _z) && (q._r == _r)); }
/* public RotationMatrix(AngleAxis angleaxis) : base(3,3) { Set(angleaxis); } */ public RotationMatrix(Quaternion q) : base(3,3) { Set(q); }
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; }
/// <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); }
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(); }
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"); } }
/// <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()); }
public AngleAxis(Quaternion q) { Set(q); }
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)); }
public void Set(Quaternion q) { this.Equals(q.RotationMatrix()); }
//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; }
/// <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; }
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; }
/// <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; }
// 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; }
/// <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)); }
public bool NotEqualTo (Quaternion q) { return ((q._x != _x) || (q._y != _y) || (q._z != _z) || (q._r != _r)); }
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; } }