public AngleAxis(Vector3D axis, float angle) { Set(axis,angle); }
public override void func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(Vector yi, Vector xp) { ThreeD_Motion_Model mm = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model; // Extract cartesian and quaternion components of xp mm.func_r(xp); mm.func_q(xp); Vector3D yWiminusrW = new Vector3D((new Vector3D(yi)) -(mm.get_rRES())); Quaternion qRES = mm.get_qRES(); Quaternion qRW = qRES.Inverse(); MatrixFixed dqRW_by_dq = MatrixFixed.dqbar_by_dq(); // Rotation RRW RotationMatrix RRW = qRW.RotationMatrix(); // Position of feature relative to robot in robot frame Vector3D zeroedyi = new Vector3D(RRW * yWiminusrW); zeroedyiRES.Update(zeroedyi.GetVNL3()); // Now calculate Jacobians // dzeroedyi_by_dyi is RRW dzeroedyi_by_dyiRES.Update(RRW); // dzeroedyi_by_dxp has 2 partitions: // dzeroedyi_by_dr (3 * 3) // dzeroedyi_by_dq (3 * 4) MatrixFixed dzeroedyi_by_dr = (MatrixFixed)(RRW); dzeroedyi_by_dr *= -1.0f; //These should be 3x4 dimension MatrixFixed dzeroedyi_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, yWiminusrW); MatrixFixed dzeroedyi_by_dq = dzeroedyi_by_dqRW * dqRW_by_dq; dzeroedyi_by_dxpRES.Update(dzeroedyi_by_dr, 0, 0); dzeroedyi_by_dxpRES.Update(dzeroedyi_by_dq, 0, 3); }
public override void func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(Vector yi, Vector xp) { Wide_Camera_Point_Feature_Measurement_Model wm = (Wide_Camera_Point_Feature_Measurement_Model)wide_model; // Extract cartesian and quaternion components of xp wm.threed_motion_model.func_r(xp); wm.threed_motion_model.func_q(xp); // Extract ri and hhati components of yi = ypi func_ri(yi); func_hhati(yi); // ri part: transformation is just the same as in the normal point case // zeroedri = RRW(rWi - rW) //commented out in original code // ri - r Vector3D yWiminusrW = new Vector3D(riRES - wm.threed_motion_model.get_rRES()); Quaternion qRW = wm.threed_motion_model.get_qRES().Inverse(); MatrixFixed dqRW_by_dq = MatrixFixed.dqbar_by_dq(); // Rotation RRW RotationMatrix RRW = qRW.RotationMatrix(); // RRW(rWi - rW) Vector3D zeroedri = new Vector3D(RRW * yWiminusrW); // Now calculate Jacobians // dzeroedri_by_dri is RRW // dzeroedri_by_dhhati = 0 MatrixFixed dzeroedri_by_dri = new MatrixFixed(RRW); // dzeroedyi_by_dxp: // dzeroedri_by_dr = -RRW // dzeroedri_by_dq = d_dq(RRW (ri - r)) MatrixFixed dzeroedri_by_dr = RRW * -1.0f; MatrixFixed dzeroedri_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, yWiminusrW); MatrixFixed dzeroedri_by_dq = dzeroedri_by_dqRW * dqRW_by_dq; // Now for the hhati part (easier...) // zeroedhhati = RRW hhati Vector3D zeroedhhati = new Vector3D(RRW * hhatiRES); // Jacobians // dzeroedhhati_by_dr = 0 // dzeroedhhati_by_dq = d_dq(RRW hhati) // dzeroedhhati_by_dhhati = RRW // dzeroedhhati_by_dri = 0 MatrixFixed dzeroedhhati_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, hhatiRES); MatrixFixed dzeroedhhati_by_dq = dzeroedhhati_by_dqRW * dqRW_by_dq; MatrixFixed dzeroedhhati_by_dhhati = new MatrixFixed(RRW); // And put it all together zeroedyiRES.Update(zeroedri.GetVNL3(), 0); zeroedyiRES.Update(zeroedhhati.GetVNL3(), 3); //cout << "Line: zeroedri = " << zeroedri << "zeroedhhati = " << zeroedhhati; dzeroedyi_by_dxpRES.Fill(0.0f); dzeroedyi_by_dxpRES.Update(dzeroedri_by_dr, 0, 0); dzeroedyi_by_dxpRES.Update(dzeroedri_by_dq, 0, 3); dzeroedyi_by_dxpRES.Update(dzeroedhhati_by_dq, 3, 3); dzeroedyi_by_dyiRES.Fill(0.0f); dzeroedyi_by_dyiRES.Update(dzeroedri_by_dri, 0, 0); dzeroedyi_by_dyiRES.Update(dzeroedhhati_by_dhhati, 3, 3); }
// @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; }
// Rotate a vector // @param v the vector to be rotated // @returns the resulting rotated vector public Vector3D Rotate(Vector3D v) { Vector3D _i = this.Imaginary(); Vector3D _n = new Vector3D(Vector3D.VectorProduct(_i, v)); Vector3D delta = new Vector3D(_r * _n - (Vector3D.VectorProduct(_n, _i))); return (new Vector3D(v + 2.0f * delta)); }
public float Multiply(LineSeg3D ls1, LineSeg3D ls2) { Vector3D d1 = new Vector3D(ls1._ep - ls1._sp); Vector3D d2 = new Vector3D(ls2._ep - ls2._sp); return (d1.GetX() * d2.GetX() + d1.GetY() * d2.GetY() + d1.GetZ() * d2.GetZ()); }
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 Vector3D Axis() { Vector3D axis = new Vector3D(0,0,0); float d0 = this[0,0], d1 = this[1,1], d2 = this[2,2]; // The trace determines the method of decomposition float rr = 1.0f + d0 + d1 + d2; if (rr > 0) { axis.SetX( this[2,1] - this[1,2] ); axis.SetY( this[0,2] - this[2,0] ); axis.SetZ( this[1,0] - this[0,1] ); } else { // Trace is less than zero, so need to determine which // major diagonal is largest if ((d0 > d1) && (d0 > d2)) { axis.SetX( 0.5f ); axis.SetY( this[0,1] + this[1,0] ); axis.SetZ( this[0,2] + this[2,0] ); } else if (d1 > d2) { axis.SetX( this[0,1] + this[1,0] ); axis.SetY( 0.5f ); axis.SetZ( this[1,2] + this[2,1] ); } else { axis.SetX( this[0,2] + this[2,0] ); axis.SetY( this[1,2] + this[2,1] ); axis.SetZ( 0.5f ); } } axis.Normalise(); return (axis); }
public Vector3D InverseRotate(Vector3D v) { return new Vector3D(this[0,0]*v.GetX()+this[1,0]*v.GetY()+this[2,0]*v.GetZ(), this[0,1]*v.GetX()+this[1,1]*v.GetY()+this[2,1]*v.GetZ(), this[0,2]*v.GetX()+this[1,2]*v.GetY()+this[2,2]*v.GetZ()); }
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 override void func_xpredef_and_dxpredef_by_dxp_and_dxpredef_by_dxpdef (Vector xp, Vector xpdef) { // Split position state into cartesian and quaternion // r0, q0: position in original frame func_r(xp); func_q(xp); Vector3D r0 = rRES; Quaternion q0 = qRES; // rn, qn: definition of new frame func_r(xpdef); func_q(xpdef); Vector3D rn = rRES; Quaternion qn = qRES; Quaternion qnbar = qn.Conjugate(); // Calculate new cartesian part of xp Vector3D Temp31a = new Vector3D(r0 - rn); // Temp31a is r0 - rn RotationMatrix Temp33a = qnbar.RotationMatrix(); Vector3D Temp31b = new Vector3D(Temp33a * Temp31a); // Copy into xpredefRES xpredefRES[0] = Temp31b.GetX(); xpredefRES[1] = Temp31b.GetY(); xpredefRES[2] = Temp31b.GetZ(); // Calculate new quaternion part of xp Quaternion Tempqa = qnbar.Multiply(q0); // Copy into xpredefRES Vector vecTempqa = Tempqa.GetRXYZ(); xpredefRES.Update(vecTempqa, 3); // Form Jacobian dxpredef_by_dxpRES dxpredef_by_dxpRES.Fill(0.0f); dxpredef_by_dxpRES.Update(Temp33a, 0, 0); MatrixFixed Temp44a = MatrixFixed.dq3_by_dq1(qnbar); dxpredef_by_dxpRES.Update(Temp44a, 3, 3); // Form Jacobian dxpredef_by_dxpdefRES dxpredef_by_dxpdefRES.Fill(0.0f); //Temp33a *= -1.0f; Temp33a = (RotationMatrix)(Temp33a * - 1.0f); dxpredef_by_dxpdefRES.Update(Temp33a, 0, 0); Temp44a = MatrixFixed.dq3_by_dq2(q0); MatrixFixed Temp44b = MatrixFixed.dqbar_by_dq(); MatrixFixed Temp44c = Temp44a * Temp44b; dxpredef_by_dxpdefRES.Update(Temp44c, 3, 3); // Top right corner of this Jacobian is tricky because we have to // differentiate a rotation matrix // Uses function that does this in bits.cc // Build this corner in Temp34a //MatrixFixed temp = new MatrixFixed(3,4); MatrixFixed Temp34a = MatrixFixed.dRq_times_a_by_dq(qnbar, Temp31a.GetVNL3()); // So far we have drN_by_dqnbar; want _by_dqn MatrixFixed Temp34b = Temp34a * Temp44b; // Finally copy into result matrix dxpredef_by_dxpdefRES.Update(Temp34b, 0, 3); // cout << "dxpredef_by_dxpdefRES" << dxpredef_by_dxpdefRES; }
/// <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 AngleAxis(Vector3D angleaxis) { Set(angleaxis); }
/// <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 RotationMatrix(Vector3D axis, float angle) : base(3,3) { Set(axis, angle); }
/// <summary> /// turns a postion and quaternion based orientation into a matrix format /// </summary> /// <param name="position"></param> /// <param name="orientation"></param> /// <returns></returns> private Matrix QuaternionToMatrixLookAt(Vector3D position, SceneLibrary.Quaternion orientation) { Microsoft.DirectX.Quaternion q_orientation = new Microsoft.DirectX.Quaternion((float)orientation.GetX(), (float)orientation.GetY(), (float)orientation.GetZ(), (float)orientation.GetR()); Matrix _matrixRotation = Matrix.RotationQuaternion(q_orientation); Vector3 camPos = new Vector3((float)position.GetX(), -(float)position.GetY(), (float)position.GetZ()); Vector3 camTY = new Vector3(_matrixRotation.M21, _matrixRotation.M22, _matrixRotation.M23); Vector3 camTZ = new Vector3(-(_matrixRotation.M31), -(_matrixRotation.M32), -(_matrixRotation.M33)); return (Matrix.LookAtLH(camPos, camPos + camTZ, camTY)); //float yaw = 0, pitch = 0, roll = 0; //DecomposeRollPitchYawZXYMatrix(_matrixRotation, ref pitch, ref yaw, ref roll); //return (Matrix.RotationYawPitchRoll((float)yaw, (float)pitch, (float)roll) * Matrix.LookAtLH(camPos, camPos + camTZ, camTY)); //return (Matrix.RotationYawPitchRoll(0, 0, (float)roll) * Matrix.LookAtLH(camPos, camPos + camTZ, camTY)); }
// Function to find a region in an image guided by current motion prediction // which doesn't overlap existing features public static bool FindNonOverlappingRegion(Scene_Single scene, Vector local_u, float delta_t, Partially_Initialised_Feature_Measurement_Model default_feature_type_for_initialisation, uint camera_width, uint camera_height, uint BOXSIZE, ref int init_feature_search_ustart, ref int init_feature_search_vstart, ref int init_feature_search_ufinish, ref int init_feature_search_vfinish, uint FEATURE_INIT_STEPS_TO_PREDICT, float FEATURE_INIT_DEPTH_HYPOTHESIS, Random rnd) { ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model(); Vector local_xv = new Vector(scene.get_xv()); for (uint i = 0; i < FEATURE_INIT_STEPS_TO_PREDICT; i++) { scene.get_motion_model().func_fv_and_dfv_by_dxv(local_xv, local_u, delta_t); local_xv.Update(scene.get_motion_model().get_fvRES()); } threed_motion_model.func_xp(local_xv); Vector local_xp = new Vector(threed_motion_model.get_xpRES()); threed_motion_model.func_r(local_xp); Vector3D rW = threed_motion_model.get_rRES(); threed_motion_model.func_q(local_xp); Quaternion qWR = threed_motion_model.get_qRES(); // yW = rW + RWR hR Vector3D hR = new Vector3D(0.0f, 0.0f, FEATURE_INIT_DEPTH_HYPOTHESIS); // Used Inverse + transpose because this was't compiling the normal way Vector3D yW = new Vector3D(rW + qWR.RotationMatrix() * hR); // Then project that point into the current camera position scene.get_motion_model().func_xp(scene.get_xv()); Fully_Initialised_Feature_Measurement_Model fully_init_fmm = (Fully_Initialised_Feature_Measurement_Model)(default_feature_type_for_initialisation.more_initialised_feature_measurement_model); Vector yWVNL = yW.GetVNL3(); fully_init_fmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yWVNL, scene.get_motion_model().get_xpRES()); // Now, this defines roughly how much we expect a feature initialised to move float suggested_u = fully_init_fmm.get_hiRES()[0]; float suggested_v = fully_init_fmm.get_hiRES()[1]; float predicted_motion_u = camera_width / 2.0f - suggested_u; float predicted_motion_v = camera_height / 2.0f - suggested_v; // So, the limits of a "safe" region within which we can initialise // features so that they end up staying within the screen // (Making the approximation that the whole screen moves like the // centre point) int safe_feature_search_ustart = (int)(-predicted_motion_u); int safe_feature_search_vstart = (int)(-predicted_motion_v); int safe_feature_search_ufinish = (int)(camera_width - predicted_motion_u); int safe_feature_search_vfinish = (int)(camera_height - predicted_motion_v); if (safe_feature_search_ustart < ((int)((BOXSIZE - 1) / 2) + 1)) safe_feature_search_ustart = (int)((BOXSIZE - 1) / 2 + 1); if (safe_feature_search_ufinish > (int)camera_width - ((int)((BOXSIZE - 1) / 2) + 1)) safe_feature_search_ufinish = (int)(camera_width - (BOXSIZE - 1) / 2 - 1); if (safe_feature_search_vstart < ((int)((BOXSIZE - 1) / 2) + 1)) safe_feature_search_vstart = (int)((BOXSIZE - 1) / 2 + 1); if (safe_feature_search_vfinish > (int)camera_height - ((int)((BOXSIZE - 1) / 2) + 1)) safe_feature_search_vfinish = (int)(camera_height - (BOXSIZE - 1) / 2 - 1); return FindNonOverlappingRegionNoPredict(safe_feature_search_ustart, safe_feature_search_vstart, safe_feature_search_ufinish, safe_feature_search_vfinish, scene, ref init_feature_search_ustart, ref init_feature_search_vstart, ref init_feature_search_ufinish, ref init_feature_search_vfinish, rnd); }
public static MatrixFixed dvnorm_by_dv(Vector3D v) { float vv = v.GetX()*v.GetX() + v.GetY()*v.GetY() + v.GetZ()*v.GetZ(); // We can use dqi_by_dqi and dqi_by_dqj functions because they are the same float[] a = new float[9]; a[0] = dqi_by_dqi(v.GetX(), vv); a[1] = dqi_by_dqj(v.GetX(), v.GetY(), vv); a[2] = dqi_by_dqj(v.GetX(), v.GetZ(), vv); a[3] = dqi_by_dqj(v.GetY(), v.GetX(), vv); a[4] = dqi_by_dqi(v.GetY(), vv); a[5] = dqi_by_dqj(v.GetY(), v.GetZ(), vv); a[6] = dqi_by_dqj(v.GetZ(), v.GetX(), vv); a[7] = dqi_by_dqj(v.GetZ(), v.GetY(), vv); a[8] = dqi_by_dqi(v.GetZ(), vv); MatrixFixed M = new MatrixFixed(3,3); return M; }
/// <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; }
//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> /// 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; }
// @param v the vector to be rotated // @returns the resulting rotated vector public Vector3D Multiply (Vector3D v) { return (this.Rotate(v)); }
/// <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)); }
// Rotate a line segment. // param l the line segment to be rotated // @returns the resulting rotated line segment public LineSeg3D Rotate(LineSeg3D l) { // Simply rotate both lines Vector3D i = this.Imaginary(); Vector3D ns = new Vector3D(Vector3D.VectorProduct(i, l.StartPoint())); Vector3D ne = new Vector3D(Vector3D.VectorProduct(i, l.EndPoint())); Vector3D deltas = new Vector3D(_r * ns - (Vector3D)(Vector3D.VectorProduct(ns, i))); Vector3D deltae = new Vector3D(_r * ne - (Vector3D)(Vector3D.VectorProduct(ne, i))); LineSeg3D result = new LineSeg3D(l.StartPoint() + 2.0f * deltas, l.EndPoint() + 2.0f * deltae); return (result); }
/// <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); }
/// <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 }
/// <summary> /// Calculate commonly used Jacobian part \partial q(\omega * \Delta t) / \partial \omega . /// </summary> /// <param name="omega"></param> /// <param name="delta_t"></param> /// <param name="dqomegadt_by_domega"></param> public void dqomegadt_by_domega(Vector3D omega, float delta_t, MatrixFixed dqomegadt_by_domega) { // Modulus float omegamod = (float)Math.Sqrt(omega.GetX() * omega.GetX() + omega.GetY() * omega.GetY() + omega.GetZ() * omega.GetZ()); // Use generic ancillary functions to calculate components of Jacobian dqomegadt_by_domega.Put(0, 0, dq0_by_domegaA(omega.GetX(), omegamod, delta_t)); dqomegadt_by_domega.Put(0, 1, dq0_by_domegaA(omega.GetY(), omegamod, delta_t)); dqomegadt_by_domega.Put(0, 2, dq0_by_domegaA(omega.GetZ(), omegamod, delta_t)); dqomegadt_by_domega.Put(1, 0, dqA_by_domegaA(omega.GetX(), omegamod, delta_t)); dqomegadt_by_domega.Put(1, 1, dqA_by_domegaB(omega.GetX(), omega.GetY(), omegamod, delta_t)); dqomegadt_by_domega.Put(1, 2, dqA_by_domegaB(omega.GetX(), omega.GetZ(), omegamod, delta_t)); dqomegadt_by_domega.Put(2, 0, dqA_by_domegaB(omega.GetY(), omega.GetX(), omegamod, delta_t)); dqomegadt_by_domega.Put(2, 1, dqA_by_domegaA(omega.GetY(), omegamod, delta_t)); dqomegadt_by_domega.Put(2, 2, dqA_by_domegaB(omega.GetY(), omega.GetZ(), omegamod, delta_t)); dqomegadt_by_domega.Put(3, 0, dqA_by_domegaB(omega.GetZ(), omega.GetX(), omegamod, delta_t)); dqomegadt_by_domega.Put(3, 1, dqA_by_domegaB(omega.GetZ(), omega.GetY(), omegamod, delta_t)); dqomegadt_by_domega.Put(3, 2, dqA_by_domegaA(omega.GetZ(), omegamod, delta_t)); }
/// <summary> /// Partial initialisation function /// Calculates the state vector \vct{y}_{pi} and Jacobians for a new /// partially-initialised feature from an initial observation. /// @param hi The initial measurement \vct{h}_i , in this case the (x,y) /// image location of the feature. /// </summary> /// <param name="hi"></param> /// <param name="xp">The robot position state \vct{x}_p from which the initial observation was made.</param> public override void func_ypi_and_dypi_by_dxp_and_dypi_by_dhi_and_Ri(Vector hi, Vector xp) { // Representation of a line here: // ypi = (rWi ) // (hLhatWi) // Form the ray (in camera co-ordinates by unprojecting this image location Vector3D hLRi = new Vector3D(((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.Unproject(hi)); //if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("debug hLRi"); // Form hLhatRi from hLRi // Normalise Vector3D hLhatRi = hLRi; hLhatRi.Normalise(); MatrixFixed dhLhatRi_by_dhLRi = MatrixFixed.dvnorm_by_dv(hLRi); //if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("debug hLhatRi" + hLhatRi); // Now convert this into a direction in world co-ordinates by rotating // Form hLhatWi from hLhatRi // Rotate ((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 hLhatWi = new Vector3D(RWR * hLhatRi); //if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("debug hLhatWi" + hLhatWi); // Extract rW from xp ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.func_r(xp); // And form ypiRES ypiRES.Update(((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_rRES().GetVNL3(), 0); ypiRES.Update(hLhatWi.GetVNL3(), 3); //if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("debug ypiRES" + ypiRES); // Form Jacobians dypi_by_dxp and dypi_by_dhi // dypi_by_dxp = (drWi_by_dr drWi_by_dq ) // (dhLhatWi_by_dr dhLhatWi_by_dq) // = (I 0 ) // (0 dhLhatWi_by_dq) // hLhatWi = RWR * hLhatRi // => dhLhatWi_by_dq = d/dq ( R(qWR) * hLhatRi) MatrixFixed dhLhatWi_by_dq = MatrixFixed.dRq_times_a_by_dq(((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_qRES(), hLhatRi); // Put dypi_by_dxp together dypi_by_dxpRES.Fill(0.0f); dypi_by_dxpRES.Put(0, 0, 1.0f); dypi_by_dxpRES.Put(1, 1, 1.0f); dypi_by_dxpRES.Put(2, 2, 1.0f); dypi_by_dxpRES.Update(dhLhatWi_by_dq, 3, 3); // cout << "dypi_by_dxpRES" << dypi_by_dxpRES; // dypi_by_dhi = (drWi_by_dhi ) // (dhLhatWi_by_dhi) // = (0 ) // (dhLhatWi_by_dhi) // hLhatWi = RWR * hLhatRi // Need to work out derivative for this // dhLhatWi_by_dhi = RWR * dhLhatRi_by_dhLRi * dhLRi_by_dhi MatrixFixed dhLhatWi_by_dhi = RWR * dhLhatRi_by_dhLRi * ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.UnprojectionJacobian(); dypi_by_dhiRES.Fill(0.0f); dypi_by_dhiRES.Update(dhLhatWi_by_dhi, 3, 0); // cout << "dypi_by_dhiRES" << dypi_by_dhiRES; // And construct Ri func_Ri(hi); /* if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("func_ypi: hi = " + hi + "," + "xp = " + xp + "," + "ypiRES = " + ypiRES); */ }
// Rotate a line segment. // @param l the line segment to be rotated // @returns the resulting rotated line segment public LineSeg3D Rotate(LineSeg3D l) { // Simply rotate both lines float angle = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z); if (angle < 1e-8) { return l; } float sn = (float)Math.Sin(angle); float cs = (float)Math.Cos(angle); Vector3D axis = new Vector3D(_x / angle, _y / angle, _z / angle); Vector3D ns = new Vector3D(Point3D.VectorProduct(axis, l.StartPoint())); Vector3D ne = new Vector3D(Point3D.VectorProduct(axis, l.EndPoint())); Vector3D deltas = new Vector3D(sn * ns + (cs - 1.0f) * (Point3D.VectorProduct(ns, axis))); Vector3D deltae = new Vector3D(sn * ne + (cs - 1.0f) * (Point3D.VectorProduct(ne, axis))); return new LineSeg3D(l.StartPoint() + deltas, l.EndPoint() + deltae); }