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;
}
