void DepthFilter::updateLineSeeds(FramePtr frame)
{
// update only a limited number of seeds, because we don't have time to do it
// for all the seeds in every frame!
size_t n_updates=0, n_failed_matches=0, n_seeds = seg_seeds_.size();
lock_t lock(seeds_mut_);
list<LineSeed>::iterator it=seg_seeds_.begin();
const double focal_length = frame->cam_->errorMultiplier2();
double px_noise = 1.0;
double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz)
while( it!=seg_seeds_.end())
{
// set this value true when seeds updating should be interrupted
if(seeds_updating_halt_)
return;
// check if seed is not already too old
if((LineSeed::batch_counter - it->batch_id) > options_.max_n_kfs) {
it = seg_seeds_.erase(it);
continue;
}
// check if segment is visible in the current image
SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse();
const Vector3d xyz_f_s(T_ref_cur.inverse()*(1.0/it->mu_s * static_cast<LineFeat*>(it->ftr)->sf) );
const Vector3d xyz_f_e(T_ref_cur.inverse()*(1.0/it->mu_e * static_cast<LineFeat*>(it->ftr)->ef) );
if( xyz_f_s.z() < 0.0 || xyz_f_e.z() < 0.0 ) {
++it; // behind the camera
continue;
}
if( !frame->cam_->isInFrame(frame->f2c(xyz_f_s).cast<int>()) ||
!frame->cam_->isInFrame(frame->f2c(xyz_f_e).cast<int>()) ) {
++it; // segment does not project in image
continue;
}
// we are using inverse depth coordinates
float z_inv_min_s = it->mu_s + sqrt(it->sigma2_s);
float z_inv_max_s = max(it->mu_s - sqrt(it->sigma2_s), 0.00000001f);
float z_inv_min_e = it->mu_e + sqrt(it->sigma2_e);
float z_inv_max_e = max(it->mu_e - sqrt(it->sigma2_e), 0.00000001f);
double z_s, z_e;
if(!matcherls_.findEpipolarMatchDirectSegmentEndpoint(
*it->ftr->frame, *frame, *it->ftr, 1.0/it->mu_s, 1.0/z_inv_min_s, 1.0/z_inv_max_s, z_s) ||
!matcherls_.findEpipolarMatchDirectSegmentEndpoint(
*it->ftr->frame, *frame, *it->ftr, 1.0/it->mu_e, 1.0/z_inv_min_e, 1.0/z_inv_max_e, z_e) )
{
it->b++; // increase outlier probability when no match was found
++it;
++n_failed_matches;
continue;
}
// compute tau
double tau_s = computeTau(T_ref_cur, static_cast<LineFeat*>(it->ftr)->sf, z_s, px_error_angle);
double tau_inverse_s = 0.5 * (1.0/max(0.0000001, z_s-tau_s) - 1.0/(z_s+tau_s));
double tau_e = computeTau(T_ref_cur, static_cast<LineFeat*>(it->ftr)->ef, z_e, px_error_angle);
double tau_inverse_e = 0.5 * (1.0/max(0.0000001, z_e-tau_e) - 1.0/(z_e+tau_e));
// update the estimate
updateLineSeed(1./z_s, tau_inverse_s*tau_inverse_s, 1./z_e, tau_inverse_e*tau_inverse_e, &*it);
++n_updates;
if(frame->isKeyframe())
{
// The feature detector should not initialize new seeds close to this location
seg_feature_detector_->setGridOccpuancy(LineFeat(matcher_.px_cur_,matcherls_.px_cur_));
}
// if the seed has converged, we initialize a new candidate point and remove the seed
if(sqrt(it->sigma2_s) < it->z_range_s/options_.seed_convergence_sigma2_thresh &&
sqrt(it->sigma2_e) < it->z_range_e/options_.seed_convergence_sigma2_thresh )
{
assert(static_cast<LineFeat*>(it->ftr)->feat3D == NULL); // TODO this should not happen anymore
Vector3d xyz_world_s(it->ftr->frame->T_f_w_.inverse() * (static_cast<LineFeat*>(it->ftr)->sf * (1.0/it->mu_s)));
Vector3d xyz_world_e(it->ftr->frame->T_f_w_.inverse() * (static_cast<LineFeat*>(it->ftr)->ef * (1.0/it->mu_e)));
LineSeg* line = new LineSeg(xyz_world_s, xyz_world_e, it->ftr);
static_cast<LineFeat*>(it->ftr)->feat3D = line;
/* FIXME it is not threadsafe to add a feature to the frame here.
if(frame->isKeyframe())
{
Feature* ftr = new PointFeat(frame.get(), matcher_.px_cur_, matcher_.search_level_);
ftr->point = point;
point->addFrameRef(ftr);
frame->addFeature(ftr);
it->ftr->frame->addFeature(it->ftr);
}
else
*/
{
seed_converged_cb_ls_(line, it->sigma2_s, it->sigma2_e); // put in candidate list
}
it = seg_seeds_.erase(it);
}
else if( isnan(z_inv_min_s) || isnan(z_inv_min_e) )
{
SVO_WARN_STREAM("z_min_s or z_min_e is NaN");
it = seg_seeds_.erase(it);
}
else
++it;
}
}
void DepthFilter::updateSeeds(FramePtr frame)
{
// update only a limited number of seeds, because we don't have time to do it
// for all the seeds in every frame!
size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size();
lock_t lock(seeds_mut_);
list<Seed>::iterator it=seeds_.begin();
const double focal_length = frame->cam_->errorMultiplier2();
double px_noise = 1.0;
double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz)
while( it!=seeds_.end())
{
// set this value true when seeds updating should be interrupted
if(seeds_updating_halt_)
return;
// check if seed is not already too old
if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) {
it = seeds_.erase(it);
continue;
}
// check if point is visible in the current image
SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse();
const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
if(xyz_f.z() < 0.0) {
++it; // behind the camera
continue;
}
if(!it->ftr->frame->cam_->isInFrame(it->ftr->frame->f2c(xyz_f).cast<int>())) {
++it; // point does not project in image
continue;
}
// we are using inverse depth coordinates
float z_inv_min = it->mu + sqrt(it->sigma2);
float z_inv_max = max(it->mu - sqrt(it->sigma2), 0.00000001f);
double z;
if(!matcher_.findEpipolarMatchDirect(
*it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max, z))
{
it->b++; // increase outlier probability when no match was found
++it;
++n_failed_matches;
continue;
}
// compute tau
double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle);
double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
// update the estimate
updateSeed(1./z, tau_inverse*tau_inverse, &*it);
++n_updates;
if(frame->isKeyframe())
{
// The feature detector should not initialize new seeds close to this location
feature_detector_->setGridOccpuancy(matcher_.px_cur_);
}
// if the seed has converged, we initialize a new candidate point and remove the seed
if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh)
{
assert(it->ftr->point == NULL); // TODO this should not happen anymore
Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));
Point* point = new Point(xyz_world, it->ftr);
it->ftr->point = point;
/* FIXME it is not threadsafe to add a feature to the frame here.
if(frame->isKeyframe())
{
Feature* ftr = new Feature(frame.get(), matcher_.px_cur_, matcher_.search_level_);
ftr->point = point;
point->addFrameRef(ftr);
frame->addFeature(ftr);
it->ftr->frame->addFeature(it->ftr);
}
else
*/
{
seed_converged_cb_(point, it->sigma2); // put in candidate list
}
it = seeds_.erase(it);
}
else if(isnan(z_inv_min))
{
SVO_WARN_STREAM("z_min is NaN");
it = seeds_.erase(it);
}
else
++it;
}
}
/**
* Must be called _after_ the child class computes dynamic_viscosity.
*/
void
NavierStokesMaterial::computeProperties()
{
for (unsigned int qp = 0; qp < _qrule->n_points(); ++qp)
{
/******* Viscous Stress Tensor *******/
// Technically... this _is_ the transpose (since we are loading these by rows)
// But it doesn't matter....
RealTensorValue grad_outer_u(_grad_u[qp], _grad_v[qp], _grad_w[qp]);
grad_outer_u += grad_outer_u.transpose();
Real div_vel = 0.0;
for (unsigned int i = 0; i < 3; ++i)
div_vel += (*_vel_grads[i])[qp](i);
// Add diagonal terms
for (unsigned int i = 0; i < 3; ++i)
grad_outer_u(i, i) -= 2.0 / 3.0 * div_vel;
grad_outer_u *= _dynamic_viscosity[qp];
_viscous_stress_tensor[qp] = grad_outer_u;
// Tabulated values of thermal conductivity vs. Temperature for air (k increases slightly with
// T):
// T (K) k (W/m-K)
// 273 0.0243
// 373 0.0314
// 473 0.0386
// 573 0.0454
// 673 0.0515
// Pr = (mu * cp) / k ==> k = (mu * cp) / Pr = (mu * gamma * cv) / Pr.
// TODO: We are using a fixed value of the Prandtl number which is
// valid for air, it may or may not depend on temperature? Since
// this is a property of the fluid, it could possibly be moved to
// the FluidProperties module...
const Real Pr = 0.71;
_thermal_conductivity[qp] = (_dynamic_viscosity[qp] * _fp.cp()) / Pr;
// Compute stabilization parameters:
// .) Compute SUPG element length scale.
computeHSUPG(qp);
// Moose::out << "_hsupg[" << qp << "]=" << _hsupg[qp] << std::endl;
// .) Compute SUPG parameter values. (Must call this after computeHSUPG())
computeTau(qp);
// Moose::out << "_tauc[" << qp << "]=" << _tauc[qp] << ", ";
// Moose::out << "_taum[" << qp << "]=" << _taum[qp] << ", ";
// Moose::out << "_taue[" << qp << "]=" << _taue[qp] << std::endl;
// .) Compute strong residual values.
computeStrongResiduals(qp);
// Moose::out << "_strong_residuals[" << qp << "]=";
// for (unsigned i=0; i<_strong_residuals[qp].size(); ++i)
// Moose::out << _strong_residuals[qp][i] << " ";
// Moose::out << std::endl;
}
}
