void ComparatorNoiseCorrector::CorrectComparatorNoise_internal(
bool verbose, bool aggressive_correction, int row_start, int row_end, bool hfonly)
{
int phase=-1;
memset(mComparator_sigs,0,mComparator_sigs_len);
memset(mComparator_noise,0,mComparator_noise_len);
memset(mComparator_hf_noise,0,mComparator_hf_noise_len);
memset(mAvg_num,0,mAvg_num_len);
memset(mComparator_mask,0,mComparator_mask_len);
memset(mComparator_hf_mask,0,mComparator_hf_mask_len);
memset(mCorrection,0,mCorrection_len);
ncomp=4;
if (row_start == -1)
{
row_start = 0;
row_end = rows;
}
// first, create the average comparator signals
// making sure to avoid pinned pixels
SumColumns(row_start, row_end);
double startTime=CNCTimer();
// subtract DC offset from average comparator signals
SetMeanToZero(mComparator_sigs);
#ifdef DBG_SAVETEMPS
DebugSaveComparatorSigs(0);
#endif
// now figure out which pair of signals go together
// this function also combines pairs of signals accordingly
// from this point forward, there are only cols*2 signals to deal with
phase = DiscoverComparatorPhase(mComparator_sigs,cols*4);
// change the data to be column major for the rest of the functions...
#ifdef DBG_SAVETEMPS
DebugSaveComparatorSigs(1);
#endif
#ifdef DBG_SAVETEMPS
DebugSaveAvgNum();
#endif
tm1 += CNCTimer()-startTime;
startTime = CNCTimer();
{
ResetMask();
double tm2_1_startTime = CNCTimer();
// now neighbor-subtract the comparator signals
NNSubtractComparatorSigs(mComparator_noise,mComparator_sigs,mComparator_mask,NNSpan,cols*ncomp,frames);
// measure noise in the neighbor-subtracted signals
CalcComparatorSigRMS(mComparator_rms,mComparator_noise,cols*ncomp,frames);
// find the noisiest 10%
MaskIQR(mComparator_mask,mComparator_rms,cols*ncomp);
#ifdef DBG_SAVETEMPS
DebugSaveComparatorNoise(0);
DebugSaveComparatorRMS(0);
DebugSaveComparatorMask(0);
#endif
// neighbor-subtract again...avoiding noisiest 10%
NNSubtractComparatorSigs(mComparator_noise,mComparator_sigs,mComparator_mask,NNSpan,cols*ncomp,frames);
// measure noise in the neighbor-subtracted signals
CalcComparatorSigRMS(mComparator_rms,mComparator_noise,cols*ncomp,frames);
ResetMask();
MaskIQR(mComparator_mask,mComparator_rms,cols*ncomp, verbose);
// neighbor-subtract again...avoiding noisiest 10%
NNSubtractComparatorSigs(mComparator_noise,mComparator_sigs,mComparator_mask,NNSpan,cols*ncomp,frames);
#ifdef DBG_SAVETEMPS
DebugSaveComparatorRMS(1);
DebugSaveComparatorNoise(1);
DebugSaveComparatorMask(1);
#endif
tm2_1 += CNCTimer()-tm2_1_startTime;
if (aggressive_correction)
{
// Newly added stuff.
// subtracts some of what we detect as comparator noise from neighbors before forming the nn average
// this cleans things up a little
double tm2_2_startTime = CNCTimer();
// make another set of noise signals that have been run through a high-pass filter
// filter low frequency noise out of noise signals
double tm2_3_startTime = CNCTimer();
memcpy(mComparator_hf_noise,mComparator_noise,sizeof(float)*cols*ncomp*frames);
HighPassFilter(mComparator_hf_noise,cols*ncomp,frames,10);
tm2_3 += CNCTimer() - tm2_3_startTime;
// neighbor-subtract again...now with some rejection of what we think the noise is
NNSubtractComparatorSigs(mComparator_noise,mComparator_sigs,mComparator_mask,NNSpan,cols*ncomp,frames,mComparator_hf_noise);
// measure noise in the neighbor-subtracted signals
CalcComparatorSigRMS(mComparator_rms,mComparator_noise,cols*ncomp,frames);
ResetMask();
// MaskIQR(mComparator_mask,mComparator_rms,cols*2, verbose);
MaskUsingDynamicStdCutoff(mComparator_mask,mComparator_rms,cols*ncomp,1.0f);
// even if some comparators didn't make the cut with the raw noise signal
// we can correct more agressively if we put the noise signal through the high pass filter
// redo the high-pass fitler
memcpy(mComparator_hf_noise,mComparator_noise,sizeof(float)*cols*ncomp*frames);
// get first principal component
GetPrincComp(mPcomp,mComparator_hf_noise,mComparator_mask,cols*ncomp,frames);
FilterUsingPrincComp(mComparator_hf_noise,mPcomp,cols*ncomp,frames);
// measure high frequency noise
CalcComparatorSigRMS(mComparator_hf_rms,mComparator_hf_noise,cols*ncomp,frames);
for ( int cndx=0;cndx < cols*ncomp;cndx++ )
{
if ( mAvg_num[cndx] == 0 )
{
mComparator_hf_mask[cndx] = 1;
}
}
MaskUsingDynamicStdCutoff(mComparator_hf_mask,mComparator_hf_rms,cols*ncomp,2.0f);
tm2_2 += CNCTimer()-tm2_2_startTime;
}
// blanks comparator signal averages that didn't have any pixels to average (probably redundant)
for ( int cndx=0;cndx < cols*ncomp;cndx++ )
{
float *cptr;
if ( mAvg_num[cndx] == 0 ) {
// get a pointer to where we will build the comparator signal average
cptr = mComparator_sigs + cndx*frames;
memset(cptr,0,sizeof(float[frames]));
}
}
tm2 += CNCTimer()-startTime;
BuildCorrection(hfonly);
#ifdef DBG_SAVETEMPS
DebugSaveCorrection(row_start, row_end);
#endif
ApplyCorrection(phase, row_start, row_end, mCorrection);
}
}
void
EstimatorFull::UpdateCamera(const Eigen::Vector3d &p_c_v, const Eigen::Quaterniond &q_c_v,
const Eigen::Matrix<double,6,6> &R,
bool absolute, bool isKeyframe, double dLambda )
{
Eigen::QuaternionAd q_c_kf;
Eigen::Vector3d p_c_kf;
if (absolute)
{
q_c_kf = Eigen::QuaternionAd(q_c_v.conjugate() * q_kf_v.toQuat().conjugate());
p_c_kf = q_kf_v.toQuat().toRotationMatrix() * (p_c_v - p_kf_v);
}
else
{
q_c_kf.q = q_c_v;
p_c_kf = p_c_v;
}
Matrix3d C_q_w_v = q_w_v.toQuat().toRotationMatrix();
Matrix3d C_q_i_w = q_i_w.toQuat().toRotationMatrix();
Matrix3d C_q_c_i = q_c_i.toQuat().toRotationMatrix();
// Build Jacobian
Matrix<double,3,28> H_p;
H_p <<
C_q_w_v.transpose() * exp(lambda),
Matrix3d::Zero(),
-C_q_w_v.transpose()*C_q_i_w.transpose()*crossMat(p_c_i)*exp(lambda),
Matrix3d::Zero(),
Matrix3d::Zero(),
(C_q_w_v.transpose()*(C_q_i_w.transpose()*p_c_i + p_i_w) + p_w_v) * exp(lambda),
C_q_w_v.transpose()*C_q_i_w.transpose()*exp(lambda),
Matrix3d::Zero(),
Matrix3d::Identity()*exp(lambda),
-C_q_w_v.transpose()*crossMat( p_i_w + C_q_i_w.transpose()*p_c_i )*exp(lambda);
Matrix<double,3,28> H_q;
H_q <<
Matrix3d::Zero(),
Matrix3d::Zero(),
C_q_c_i,
Matrix3d::Zero(),
Matrix3d::Zero(),
Vector3d::Zero(),
Matrix3d::Zero(),
Matrix3d::Identity(),
Matrix3d::Zero(),
C_q_c_i*C_q_i_w;
Matrix<double,6,28> H;
H << H_p, H_q;
// Calculate residual
Vector3d r_p = p_c_kf - ( C_q_w_v.transpose()*(p_i_w + C_q_i_w.transpose()*p_c_i) + p_w_v ) * exp(lambda);
Quaterniond r_q = (q_c_kf.toQuat()*( q_c_i.toQuat()*q_i_w.toQuat()*q_w_v.toQuat() ).conjugate()).conjugate();
Matrix<double,6,1> r;
r << r_p, 2*r_q.x(), 2*r_q.y(), 2*r_q.z();
// Calculate kalmn gain
Matrix<double,6,6> S = H*P*H.transpose() + R;
//K = P*H.transpose()*S^-1;
//TODO: use cholsky to solve K*S = P*H.transposed()?
// (K*S)' = (P*H')'
// S'*K' = H*P'
// K' = S.transpose.ldlt().solve(H*P.transpose)
Matrix<double,28,6> K = (S.ldlt().solve(H*P)).transpose(); // P and S are symmetric
//Matrix<double,28,6> K = P*H.transpose()*S.inverse();//S.lu().solve(H*P).transpose();
Matrix<double,28,1> x_error = K*r;
// Apply Kalman gain
ApplyCorrection( x_error );
Matrix<double,28,28> T = Matrix<double,28,28>::Identity() - K*H;
P = T*P*T.transpose() + K*R*K.transpose();
// Symmetrize
P = (P + P.transpose())/2.0;
if (isKeyframe)
{
// Add new keyframe from current position/camera pose
UpdateKeyframe( );
// Add noise to lambda
P(15,15) += dLambda;
// Update keyframe reference, if given path is absolute
if (absolute)
{
p_kf_v = p_c_v;
q_kf_v.q = q_c_v;
}
}
}
