inline void r_and_t(MatrixXf &rot_cw, VectorXf &pos_cw,MatrixXf start_points, MatrixXf end_points, MatrixXf P1w,MatrixXf P2w,MatrixXf initRot_cw,VectorXf initPos_cw, int maxIterNum,float TerminateTh,int nargin) { if(nargin<6) { return; } if(nargin<8) { maxIterNum = 8; TerminateTh = 1e-5; } int n = start_points.cols(); if(n != end_points.cols() || n!= P1w.cols() || n!= P2w.cols()) { return; } if(n<4) { return; } //first compute the weight of each line and the normal of //the interpretation plane passing through to camera center and the line VectorXf w = VectorXf::Zero(n); MatrixXf nc = MatrixXf::Zero(3,n); for(int i = 0 ; i < n ; i++) { //the weight of a line is the inverse of its image length w[i] = 1/(start_points.col(i)-end_points.col(i)).norm(); vfloat3 v1 = start_points.col(i); vfloat3 v2 = end_points.col(i); vfloat3 temp = v1.cross(v2); nc.col(i) = temp/temp.norm(); } MatrixXf rot_wc = initPos_cw.transpose(); MatrixXf pos_wc = - initRot_cw.transpose() * initPos_cw; for(int iter = 1 ; iter < maxIterNum ; iter++) { //construct the equation (31) MatrixXf A = MatrixXf::Zero(6,7); MatrixXf C = MatrixXf::Zero(3,3); MatrixXf D = MatrixXf::Zero(3,3); MatrixXf F = MatrixXf::Zero(3,3); vfloat3 c_bar = vfloat3(0,0,0); vfloat3 d_bar = vfloat3(0,0,0); for(int i = 0 ; i < n ; i++) { //for first point on line vfloat3 Pi = rot_wc * P1w.col(i); vfloat3 Ni = nc.col(i); float wi = w[i]; vfloat3 bi = Pi.cross(Ni); C = C + wi*Ni*Ni.transpose(); D = D + wi*bi*bi.transpose(); F = F + wi*Ni*bi.transpose(); vfloat3 tempi = Pi + pos_wc; float scale = Ni.transpose() * tempi; scale *= wi; c_bar = c_bar + scale * Ni; d_bar = d_bar + scale*bi; //for second point on line Pi = rot_wc * P2w.col(i); Ni = nc.col(i); wi = w[i]; bi = Pi.cross(Ni); C = C + wi*Ni*Ni.transpose(); D = D + wi*bi*bi.transpose(); F = F + wi*Ni*bi.transpose(); scale = (Ni.transpose() * (Pi + pos_wc)); scale *= wi; c_bar = c_bar + scale * Ni; d_bar = d_bar + scale * bi; } A.block<3,3>(0,0) = C; A.block<3,3>(0,3) = F; (A.col(6)).segment(0,2) = c_bar; A.block<3,3>(3,0) = F.transpose(); A.block<3,3>(2,2) = D; (A.col(6)).segment(3,5) = d_bar; //sovle the system by using SVD; JacobiSVD<MatrixXf> svd(A, ComputeThinU | ComputeThinV); VectorXf vec(7); //the last column of Vmat; vec = (svd.matrixV()).col(6); //the condition that the last element of vec should be 1. vec = vec/vec[6]; //update the rotation and translation parameters; vfloat3 dT = vec.segment(0,2); vfloat3 dOmiga = vec.segment(3,5); MatrixXf rtemp(3,3); rtemp << 1, -dOmiga[2], dOmiga[1], dOmiga[2], 1, -dOmiga[1], -dOmiga[1], dOmiga[0], 1; rot_wc = rtemp * rot_wc; //newRot_wc = ( I + [dOmiga]x ) oldRot_wc //may be we can compute new R using rodrigues(r+dr) pos_wc = pos_wc + dT; if(dT.norm() < TerminateTh && dOmiga.norm() < 0.1*TerminateTh) { break; } } rot_cw = rot_wc.transpose(); pos_cw = -rot_cw * pos_wc; }
bool Reed_Solomon::decode(const bvec &coded_bits, const ivec &erasure_positions, bvec &decoded_message, bvec &cw_isvalid) { bool decoderfailure, no_dec_failure; int j, i, kk, l, L, foundzeros, iterations = floor_i(static_cast<double>(coded_bits.length()) / (n * m)); bvec mbit(m * k); decoded_message.set_size(iterations * k * m, false); cw_isvalid.set_length(iterations); GFX rx(q, n - 1), cx(q, n - 1), mx(q, k - 1), ex(q, n - 1), S(q, 2 * t), Xi(q, 2 * t), Gamma(q), Lambda(q), Psiprime(q), OldLambda(q), T(q), Omega(q); GFX dummy(q), One(q, (char*)"0"), Omegatemp(q); GF delta(q), tempsum(q), rtemp(q), temp(q), Xk(q), Xkinv(q); ivec errorpos; if ( erasure_positions.length() ) { it_assert(max(erasure_positions) < iterations*n, "Reed_Solomon::decode: erasure position is invalid."); } no_dec_failure = true; for (i = 0; i < iterations; i++) { decoderfailure = false; //Fix the received polynomial r(x) for (j = 0; j < n; j++) { rtemp.set(q, coded_bits.mid(i * n * m + j * m, m)); rx[j] = rtemp; } // Fix the Erasure polynomial Gamma(x) // and replace erased coordinates with zeros rtemp.set(q, -1); ivec alphapow = - ones_i(2); Gamma = One; for (j = 0; j < erasure_positions.length(); j++) { rx[erasure_positions(j)] = rtemp; alphapow(1) = erasure_positions(j); Gamma *= (One - GFX(q, alphapow)); } //Fix the syndrome polynomial S(x). S.clear(); for (j = 1; j <= 2 * t; j++) { S[j] = rx(GF(q, b + j - 1)); } // calculate the modified syndrome polynomial Xi(x) = Gamma * (1+S) - 1 Xi = Gamma * (One + S) - One; // Apply Berlekam-Massey algorithm if (Xi.get_true_degree() >= 1) { //Errors in the received word // Iterate to find Lambda(x), which hold all error locations kk = 0; Lambda = One; L = 0; T = GFX(q, (char*)"-1 0"); while (kk < 2 * t) { kk = kk + 1; tempsum = GF(q, -1); for (l = 1; l <= L; l++) { tempsum += Lambda[l] * Xi[kk - l]; } delta = Xi[kk] - tempsum; if (delta != GF(q, -1)) { OldLambda = Lambda; Lambda -= delta * T; if (2 * L < kk) { L = kk - L; T = OldLambda / delta; } } T = GFX(q, (char*)"-1 0") * T; } // Find the zeros to Lambda(x) errorpos.set_size(Lambda.get_true_degree()); foundzeros = 0; for (j = q - 2; j >= 0; j--) { if (Lambda(GF(q, j)) == GF(q, -1)) { errorpos(foundzeros) = (n - j) % n; foundzeros += 1; if (foundzeros >= Lambda.get_true_degree()) { break; } } } if (foundzeros != Lambda.get_true_degree()) { decoderfailure = true; } else { // Forney algorithm... //Compute Omega(x) using the key equation for RS-decoding Omega.set_degree(2 * t); Omegatemp = Lambda * (One + Xi); for (j = 0; j <= 2 * t; j++) { Omega[j] = Omegatemp[j]; } //Find the error/erasure magnitude polynomial by treating them the same Psiprime = formal_derivate(Lambda*Gamma); errorpos = concat(errorpos, erasure_positions); ex.clear(); for (j = 0; j < errorpos.length(); j++) { Xk = GF(q, errorpos(j)); Xkinv = GF(q, 0) / Xk; // we calculate ex = - error polynomial, in order to avoid the // subtraction when recunstructing the corrected codeword ex[errorpos(j)] = (Xk * Omega(Xkinv)) / Psiprime(Xkinv); if (b != 1) { // non-narrow-sense code needs corrected error magnitudes int correction_exp = ( errorpos(j)*(1-b) ) % n; ex[errorpos(j)] *= GF(q, correction_exp + ( (correction_exp < 0) ? n : 0 )); } } //Reconstruct the corrected codeword. // instead of subtracting the error/erasures, we calculated // the negative error with 'ex' above cx = rx + ex; //Code word validation S.clear(); for (j = 1; j <= 2 * t; j++) { S[j] = cx(GF(q, b + j - 1)); } if (S.get_true_degree() >= 1) { decoderfailure = true; } } } else { cx = rx; decoderfailure = false; } //Find the message polynomial mbit.clear(); if (decoderfailure == false) { if (cx.get_true_degree() >= 1) { // A nonzero codeword was transmitted if (systematic) { for (j = 0; j < k; j++) { mx[j] = cx[j]; } } else { mx = divgfx(cx, g); } for (j = 0; j <= mx.get_true_degree(); j++) { mbit.replace_mid(j * m, mx[j].get_vectorspace()); } } } else { //Decoder failure. // for a systematic code it is better to extract the undecoded message // from the received code word, i.e. obtaining a bit error // prob. p_b << 1/2, than setting all-zero (p_b = 1/2) if (systematic) { mbit = coded_bits.mid(i * n * m, k * m); } else { mbit = zeros_b(k); } no_dec_failure = false; } decoded_message.replace_mid(i * m * k, mbit); cw_isvalid(i) = (!decoderfailure); } return no_dec_failure; }