void Himmel::update()
{
AbstractHimmel::update();
updateSeed();
if(isDirty())
{
const t_aTime atime = t_aTime::fromTimeF(*getTime());
astro()->update(atime);
osg::Vec3f sunv = astro()->getSunPosition(false);
u_sun->set(sunv);
osg::Vec3f sunrv = astro()->getSunPosition(true);
u_sunr->set(sunrv);
u_time->set(static_cast<float>(getTime()->getf()));
if(m_starmap)
m_starmap->update(*this);
if(m_moon)
{
m_moon->update(*this);
//m_moonGlare->update(*this);
}
if(m_stars)
m_stars->update(*this);
if(m_atmosphere)
m_atmosphere->update(*this);
if(m_highLayer)
m_highLayer->update(*this);
if(m_dubeLayer)
m_dubeLayer->update(*this);
dirty(false);
}
}
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;
}
}
