C++ mat::clear方法代码示例

void solve (int x) {
    ans.clear();
    ans.l1 = 0;
    ans.l2 = t;
    ans.a[0][0] = 1;
    f.l1 = f.l2 = t;
    //f.print();
    while (x) {
        if (x % 2) ans = ans * f;
        f = f * f;
        x = x/2;
    }
    //ans = ans * f;
}

 void reparametrize_func(const vec & lmk, vec & lnew, mat & LNEW_lmk) const {
   vec7 lmk1;
   vec7 lmk2;
   vec3 euc1;
   vec3 euc2;
   mat EUC1_lmk1(3,7);
   mat EUC2_lmk2(3,7);
   LNEW_lmk.clear();
   subrange(lmk1,0,3) = subrange(lmk,0,3);
   subrange(lmk1,3,7) = subrange(lmk,3,7);
   subrange(lmk2,0,3) = subrange(lmk,0,3);
   subrange(lmk2,3,7) = subrange(lmk,7,11);
   lmkAHP::ahp2euc(lmk1,euc1,EUC1_lmk1);
   lmkAHP::ahp2euc(lmk2,euc2,EUC2_lmk2);
   subrange(lnew,0,3) = euc1;
   subrange(lnew,3,6) = euc2;
   subrange(LNEW_lmk,0,3,0,7)  = EUC1_lmk1;
   subrange(LNEW_lmk,3,6,0,3)  = subrange(EUC2_lmk2,0,3,0,3);
   subrange(LNEW_lmk,3,6,7,11) = subrange(EUC2_lmk2,0,3,3,7);
 }

int main () {
    freopen ("sequence.in", "r", stdin);
    freopen ("sequence.out", "w", stdout);
    f.clear();
    cin >> m >> n;
    for (int i=1; i<=m; i++) scanf("%s", str[i]);
    for (int i=1; i<=m; i++)
        for (int j=0; str[i][j]; j++)
            s[i] += str[i][j];

    for (int i=1; i<=m; i++)
        for (int j=1; j<=m; j++)
            if (i != j && sub (i, j))
                del[i] = 1;

    for (int i=1; i<=m; i++)
        if (!del[i]) {
            int now = 0;
            for (int j=0; j<s[i].size(); j++) {
                if (trie[now].mark) break;
                if (!trie[now].a[turn(s[i][j])])
                    trie[now].a[turn(s[i][j])] = ++t;
                now = trie[now].a[turn(s[i][j])];
            }
            trie[now].mark = 1;
        }

    int q[101], head = 0, tail = 0, now = 0;
    memset (q , 0, sizeof (q));
    for (int i=0; i<4; i++)
        if (now = trie[0].a[i]) {
            if (!trie[now].mark) {
                f.a[0][now] ++;
                q[tail] = now;
                tail++;
            }
        }
        else f.a[0][0] ++;

    while (head < tail) {
        for (int i=0; i<4; i++)
            if (trie[q[head]].a[i] && !trie[trie[q[head]].a[i]].mark) {
                int now = trie[q[head]].fail;
                while (now && !trie[now].a[i])
                    now = trie[now].fail;
                trie[trie[q[head]].a[i]].fail = trie[now].a[i];
                q[tail] = trie[q[head]].a[i];

                tail++;
            }
        head++;
    }

    for (int i=1; i<=t; i++)
        for (int j=0; j<4; j++)
            if (!trie[i].mark) {
                int cur = i;
                while (cur && !trie[cur].a[j]) cur = trie[cur].fail;
                if (!trie[trie[cur].a[j]].mark)
                    f.a[i][trie[cur].a[j]]++;
            }


    //ans = f;
    //for (int i=1; i<n; i++)
    //    ans = mul (ans, f);

    solve (n);
    for (int i=0; i<=t; i++)
        an += ans.a[0][i];
    cout << an % 100000 << endl;
}

//.........这里部分代码省略.........
			abnew = ab + abi; //          acc bias
			wbnew = wb + wbi; //          gyro bias
			gnew = g; //                 gravity does not change
			
			// normalize quaternion
			ublasExtra::normalizeJac(qnew, QNORM_qnew);
			ublasExtra::normalize(qnew);

			// Put it all together - this is the output state
			unsplitState(pnew, qnew, vnew, abnew, wbnew, gnew, _xnew);


			// Now on to the Jacobian...
			// Identity is a good place to start since overall structure is like this
			// var    |  p       q        v        ab       wb       g
			//    pos |  0       3        7        10       13       16
			// -------+----------------------------------------------------
			//#if AVGSPEED
			// p   0  |  I  VNEW_q*dt/2  I*dt -R*dt*dt/2    0     I*dt*dt/2
			//#else
			// p   0  |  I       0       I*dt      0        0        0
			//#endif
			// q   3  |  0     QNEW_q     0        0      QNEW_wb    0
			// v   7  |  0     VNEW_q     I      -R*dt      0       I*dt
			// ab  10 |  0       0        0        I        0        0
			// wb  13 |  0       0        0        0        I        0
			// g   16 |  0       0        0        0        0        I

			_XNEW_x.assign(identity_mat(state.size()));

			// Fill in XNEW_v: VNEW_g and PNEW_v = I * dt
			identity_mat I(3);
			Idt = I * _dt;
			mat Iz; if (g_size == 1) { Iz.resize(3,1); Iz.clear(); Iz(2,0)=1; } else { Iz = I; }
			mat Izdt = Iz * _dt;
			subrange(_XNEW_x, 0, 3, 7, 10) = Idt;
			#if AVGSPEED
			subrange(_XNEW_x, 0, 3, 16, 16+g_size) = Izdt*_dt/2;
			#endif
			subrange(_XNEW_x, 7, 10, 16, 16+g_size) = Izdt;

			// Fill in QNEW_q
			// qnew = qold ** qwdt  ( qnew = q1 ** q2 = qProd(q1, q2) in rtslam/quatTools.hpp )
			qProd_by_dq1(qwdt, QNEW_q);
			subrange(_XNEW_x, 3, 7, 3, 7) = prod(QNORM_qnew, QNEW_q);

			// Fill in QNEW_wb
			// QNEW_wb = QNEW_qwdt * QWDT_wdt * WDT_w * W_wb
			//         = QNEW_qwdt * QWDT_w * W_wb
			//         = QNEW_qwdt * QWDT_w * (-1)
			qProd_by_dq2(q, QNEW_qwdt);
			// Here we get the derivative of qwdt wrt wtrue, so we consider dt = 1 and call for the derivative of v2q() with v = w*dt
//			v2q_by_dv(wtrue, QWDT_w);
			v2q_by_dv(wtrue*_dt, QWDT_w); QWDT_w *= _dt;
			QNEW_w = prod ( QNEW_qwdt, QWDT_w);
			subrange(_XNEW_x, 3, 7, 13, 16) = -prod(QNORM_qnew,QNEW_w);

			// Fill VNEW_q
			// VNEW_q = d(R(q)*v) / dq
			rotate_by_dq(q, am-ab, VNEW_q); VNEW_q *= _dt;
			subrange(_XNEW_x, 7, 10, 3, 7) = VNEW_q;
			#if AVGSPEED
			subrange(_XNEW_x, 0, 3, 3, 7) = VNEW_q*_dt/2;
			#endif

			// Fill in VNEW_ab

//.........这里部分代码省略.........
			 *						    0             3       7     10        13             16                20          |
			 *      	-------------------------------------------------------------------------------------------+------
       *             ____________________________________________________________________
			 *  XNEW_x = [ |                      PNEW_v                                      |                ] |  0  p     \|
			 *           [ |                           QNEW_w                                 |                ] |  3  q      |
			 *           [ |                      I_3                                         |                ] |  7  v      | Robot State
			 *           [ |                            I_3                                   |                ] |  10 w      |
			 *           [ | [-----PBASENEW_f----]              PBASENEW_pbase                |                ] |  13 pBase  |
			 *           [ |            QBASENEW_q                             QBASENEW_qbase |                ] |  16 qBase /|
			 *           [ |__________________________________________________________________|                ] |
			 *           [  LNEW_p     LNEW_q                                                     LNEW_lold    ] |  20 lnew
			 *
			 * -----------------------------------------------------------------------------
			 *
			 * The Jacobian XNEW_pert is built with
			 *          			 vi     wi   |
			 *                 0      3    |
			 *       			-----------------+------
			 *                ____________
			 * XNEW_pert = 	[|            |] | 0  p     \|
			 * 					   	[|            |] | 3  q      |
			 * 							[| I_3        |] | 7  v      | Robot State
			 * 							[|        I_3 |] | 10 w      |
			 * 							[|            |] | 7  pBase  |
			 * 							[|____________|] | 10 qBase /|
			 * 							[              ] | 7  lnew
			 * this Jacobian is however constant and is computed once at Construction time.
			 *
			 * NOTE: The also constant perturbation matrix:
			 *    Q = XNEW_pert * perturbation.P * trans(XNEW_pert)
			 * could be built also once after construction with computeStatePerturbation().
			 * This is up to the user -- if nothing is done, Q will be computed at each iteration.
			 * -----------------------------------------------------------------------------
			 */

			lastJump = ublas::subrange(_x, 0, 7) ; // will be used in the reframe process of : the robot, the landmarks.


			// TODO the internals jacobians (of fix size)
			//  can be defined in the class definition,
			//  or the RobotAbstract class.

			// used variables :
			// 1- robot at time t
			vec3 p, v, w, pbase;
			vec4 q, qbase;
			// 2- robot at time t+1
			vec3 pnew, vnew, wnew, pbasenew;
			vec4 qnew, qbasenew;
			// 3- jacobians from t to t+1
			mat   PBASENEW_f(3, 7);
			mat33 PBASENEW_pbase ;
			mat44 QBASENEW_q, QBASENEW_qbase;
			identity_mat I_3(3);

			// split robot state vector (F is the reference frame change between t and t+1)
			splitState(_x, p, q, v, w, pbase, qbase);
			lastJump = ublas::subrange(_x, 0, 7) ;

			// split perturbation vector
			vec3 vi, wi;
			splitControl(_n, vi, wi);

			// predict each part of the state, give or build non-trivial Jacobians
			// 1- PQVW at t+1
			pnew      = v   * _dt; // FIXME reframe the new position and quaternion (position R_t relative to frame R_t-1)
			PNEW_v    = I_3 * _dt;
			quaternion::v2q(w * _dt, qnew, QNEW_wdt); // FiXME _w or _wdt
			vnew      = v + vi;
			wnew      = w + wi;
			// 2- PQ-of-Base at t+1 (reframe the base frame)
			quaternion::eucToFrame(lastJump, pbase, pbasenew, PBASENEW_f, PBASENEW_pbase) ; // pbase
			quaternion::qProd(qbase, q, qbasenew, QBASENEW_qbase, QBASENEW_q)      ; // qbase

			// Re-compose state - this is the output state.
			unsplitState(pnew, qnew, vnew, wnew, pbasenew, qbasenew, _xnew); // Robot state

			// Build transition Jacobian matrix XNEW_x
			_XNEW_x.clear() ;
			project(_XNEW_x, range(0 , 3 ), range(7 , 10)) = PNEW_v        ;
			project(_XNEW_x, range(3 , 7 ), range(10, 13)) = QNEW_wdt      ; // FIXME jacobian wrt w or wdt
			project(_XNEW_x, range(7 , 10), range(7 , 10)) = I_3           ;
			project(_XNEW_x, range(10, 13), range(10, 13)) = I_3           ;
			project(_XNEW_x, range(13, 16), range(0 , 7 )) = PBASENEW_f    ;
			project(_XNEW_x, range(13, 16), range(13, 16)) = PBASENEW_pbase;
			project(_XNEW_x, range(16, 20), range(3 , 7 )) = QBASENEW_q    ;
			project(_XNEW_x, range(16, 20), range(16, 20)) = QBASENEW_qbase;

			// NOW the landmarks are defined in the frame F = [{0,0,0, 1,0,0,0} - lastJump],
			// so we will reframe all the landmarks.


			/*
			 * We are normally supposed here to build the perturbation Jacobian matrix XNEW_pert.
			 * NOTE: XNEW_pert is constant and it has been build in the constructor.
			 *
			 * \sa See computePertJacobian() for more info.
			 */

		}