std::auto_ptr<Observation> RIFReader::readObservation_()
{
NUKLEI_TRACE_BEGIN();
if (rows_ == 0 || columns_ == 0) NUKLEI_THROW("Reader does not seem inited.");
for (;;)
{
if (currentIndex_ >= rows_*columns_)
return std::auto_ptr<Observation>();
unsigned index = currentIndex_;
currentIndex_++;
if (flags_[index] == true) continue;
std::auto_ptr<RIFObservation> observation(new RIFObservation);
Vector3 loc(x_[index], y_[index], z_[index]);
observation->setLoc(loc);
RGBColor c(rgb_[index]);
observation->setColor(c);
return std::auto_ptr<Observation>(observation);
}
return std::auto_ptr<Observation>();
NUKLEI_TRACE_END();
}
HSVConeColor::HSVConeColor(const Color& c)
{
const RGBColor* rgbc = NULL;
const HSVColor* hsvc = NULL;
const HSVConeColor* hsvcc = NULL;
if ( (rgbc = dynamic_cast<const RGBColor*>(&c)) != NULL)
{
HSVColor hsv(c);
HSVConeColor hsvCone(hsv);
c_ = hsvCone.c_;
valueWeight_ = hsvCone.valueWeight_;
}
else if ( (hsvc = dynamic_cast<const HSVColor*>(&c)) != NULL)
{
Vector3 ec;
ec.X() = std::cos(hsvc->H()) * hsvc->S() * hsvc->V();
ec.Y() = std::sin(hsvc->H()) * hsvc->S() * hsvc->V();
NUKLEI_RANGE_CHECK(HSV_METRIC_VALUE_WEIGHT, 0, 1);
ec.Z() = hsvc->V()*HSV_METRIC_VALUE_WEIGHT;
c_ = ec;
valueWeight_ = HSV_METRIC_VALUE_WEIGHT;
}
else if ( (hsvcc = dynamic_cast<const HSVConeColor*>(&c)) != NULL)
{
c_ = hsvcc->c_;
valueWeight_ = hsvcc->valueWeight_;
}
else NUKLEI_THROW("Unknown color type");
assertConsistency();
}
HSVColor::HSVColor(const Color& c)
{
const RGBColor* rgbc = NULL;
const HSVColor* hsvc = NULL;
const HSVConeColor* hsvcc = NULL;
if ( (rgbc = dynamic_cast<const RGBColor*>(&c)) != NULL)
{
appear_t rgbMin, rgbMax, delta;
appear_t r = rgbc->R(), g = rgbc->G(), b = rgbc->B();
rgbMin = std::min(r,std::min(g,b));
rgbMax = std::max(r,std::max(g,b));
delta = rgbMax - rgbMin;
V() = rgbMax;
S() = 0;
if (rgbMax > 0)
S() = delta / rgbMax;
// fixme: handle floating point check here.
NUKLEI_ASSERT( 0 <= S() && S() <= 1 );
H() = 0;
if (delta > 0)
{
if (rgbMax == r && rgbMax != g)
H() += (g - b) / delta;
if (rgbMax == g && rgbMax != b)
H() += (2 + (b - r) / delta);
if (rgbMax == b && rgbMax != r)
H() += (4 + (r - g) / delta);
H() *= 60;
if (H() < 0)
H() += 360;
}
H() *= M_PI / 180;
}
else if ( (hsvc = dynamic_cast<const HSVColor*>(&c)) != NULL) c_ = hsvc->c_;
else if ( (hsvcc = dynamic_cast<const HSVConeColor*>(&c)) != NULL)
{
NUKLEI_ASSERT(hsvcc->W() > 0);
V() = hsvcc->WV() / hsvcc->W();
if (V() > 0)
{
S() = std::sqrt( std::pow(hsvcc->SCosH(), 2) + std::pow(hsvcc->SSinH(), 2) ) / V();
if (hsvcc->SSinH() >= 0) H() = ACos(hsvcc->SCosH() / (V()*S()));
else H() = 2*M_PI-ACos(hsvcc->SCosH() / (V()*S()));
}
else if (V() == 0)
{
S() = 1;
H() = 0;
}
else NUKLEI_ASSERT(false);
}
else NUKLEI_THROW("Unknown color type");
assertConsistency();
}
kernel::base::ptr readSingleObservation(const std::string &s,
Observation::Type& t)
{
KernelCollection kc;
readObservations(s, kc, t);
if (kc.size() != 1)
NUKLEI_THROW("File `" << s << "' does not contain a single observation.");
return kc.front().clone();
}
double ColorPairDescriptor::distanceTo(const Descriptor &d) const
{
NUKLEI_TRACE_BEGIN();
const ColorDescriptor* cd = dynamic_cast<const ColorDescriptor*>(&d);
if (cd != NULL)
{
HSVConeColor c1(getLeftColor());
HSVConeColor c2(getRightColor());
HSVConeColor c3(cd->getColor());
return std::min(c1.distanceTo(c3),
c2.distanceTo(c3));
}
NUKLEI_THROW("Not implemented.");
NUKLEI_TRACE_END();
}
static kernel::se3
mcmc(const KernelCollection& objectEvidence,
const KernelCollection& sceneEvidence,
const coord_t objectSize,
const int n)
{
kernel::se3 current, best;
weight_t currentWeight = 0;
best.setWeight(currentWeight);
metropolisHastings(objectEvidence, sceneEvidence,
current, currentWeight, 1, true, n);
const int nSteps = 10*n;
for (int i = 0; i < nSteps; i++)
{
{
// begin and end bandwidths for the local proposal
coord_t bLocH = objectSize/10;
coord_t eLocH = objectSize/40;
coord_t bOriH = .1;
coord_t eOriH = .02;
unsigned e = nSteps-1;
current.setLocH(double(e-i)/e * bLocH +
double(i)/e * eLocH);
current.setOriH(double(e-i)/e * bOriH +
double(i)/e * eOriH);
if (current.loc_h_ <= 0)
NUKLEI_THROW("Unexpected value for curent.loc_h_");
}
metropolisHastings(objectEvidence, sceneEvidence,
current, currentWeight,
Ti(i, nSteps/5), false, n);
if (currentWeight > best.getWeight())
{
best = current;
best.setWeight(currentWeight);
}
}
return best;
}
RGBColor::RGBColor(const Color&c)
{
const RGBColor* rgbc;
const HSVColor* hsvc;
const HSVConeColor* hsvcc;
if ( (rgbc = dynamic_cast<const RGBColor*>(&c)) != NULL) c_ = rgbc->c_;
else if ( (hsvc = dynamic_cast<const HSVColor*>(&c)) != NULL)
{
appear_t h = hsvc->H() * 180 / M_PI, s = hsvc->S(), v = hsvc->V();
NUKLEI_RANGE_CHECK(h, 0, 360);
int i;
appear_t f, p, q, t, hTemp;
if(s == 0.0)
{
R() = G() = B() = v;
return;
}
hTemp = h/60.0f;
i = (int)std::floor( hTemp );
f = hTemp - i;
p = v * ( 1 - s );
q = v * ( 1 - s * f );
t = v * ( 1 - s * ( 1 - f ) );
switch( i )
{
case 0:{R() = v;G() = t;B() = p;break;}
case 1:{R() = q;G() = v;B() = p;break;}
case 2:{R() = p;G() = v;B() = t;break;}
case 3:{R() = p;G() = q;B() = v;break;}
case 4:{R() = t;G() = p;B() = v;break;}
default:{R() = v;G() = p;B() = q;break;}
}
}
else if ( (hsvcc = dynamic_cast<const HSVConeColor*>(&c)) != NULL)
{
HSVColor tmp(*hsvcc);
RGBColor tmp2(tmp);
c_ = tmp2.getRGB();
}
else NUKLEI_THROW("Unknown color type");
assertConsistency();
}
std::auto_ptr<ObservationWriter>
ObservationWriter::createWriter(const std::string& arg, const Observation::Type t)
{
NUKLEI_TRACE_BEGIN();
std::auto_ptr<ObservationWriter> writer;
switch (t)
{
case Observation::COVIS3D:
{
writer.reset(new CoViS3DXMLWriter(arg));
break;
}
case Observation::NUKLEI:
{
writer.reset(new NukleiWriter(arg));
break;
}
case Observation::OSUTXT:
{
NUKLEI_THROW("Not implemented.");
//writer.reset(new OsuTxtWriter(arg));
break;
}
case Observation::PCD:
{
writer.reset(new PCDWriter(arg));
break;
}
case Observation::PLY:
{
writer.reset(new PLYWriter(arg));
break;
}
case Observation::RIF:
{
NUKLEI_THROW("Not implemented.");
//writer.reset(new OsuTxtWriter(arg));
break;
}
case Observation::CRD:
{
writer.reset(new CrdWriter(arg));
break;
}
case Observation::OFF:
{
writer.reset(new OffWriter(arg));
break;
}
case Observation::SERIAL:
{
writer.reset(new KernelWriter(arg));
break;
}
case Observation::BUILTINVTK:
{
NUKLEI_THROW("Not implemented.");
//writer.reset(new OsuTxtWriter(arg));
break;
}
case Observation::TXT:
{
writer.reset(new TxtWriter(arg));
break;
}
case Observation::IIS:
{
writer.reset(new IisWriter(arg));
break;
}
default:
{
NUKLEI_THROW("Unknown format.");
break;
}
}
writer->init();
return writer;
NUKLEI_TRACE_END();
}
std::auto_ptr<ObservationReader>
ObservationReader::createReader(const std::string& arg, const Observation::Type t)
{
NUKLEI_TRACE_BEGIN();
std::auto_ptr<ObservationReader> reader;
switch (t)
{
case Observation::COVIS3D:
{
reader.reset(new CoViS3DReader(arg));
break;
}
case Observation::NUKLEI:
{
reader.reset(new NukleiReader(arg));
break;
}
case Observation::OSUTXT:
{
reader.reset(new OsuTxtReader(arg));
break;
}
case Observation::PCD:
{
reader.reset(new PCDReader(arg));
break;
}
case Observation::PLY:
{
reader.reset(new PLYReader(arg));
break;
}
case Observation::RIF:
{
reader.reset(new RIFReader(arg));
break;
}
case Observation::CRD:
{
reader.reset(new CrdReader(arg));
break;
}
case Observation::OFF:
{
reader.reset(new OffReader(arg));
break;
}
case Observation::SERIAL:
{
reader.reset(new KernelReader(arg));
break;
}
case Observation::BUILTINVTK:
{
reader.reset(new BuiltinVTKReader(arg));
break;
}
case Observation::TXT:
{
reader.reset(new TxtReader(arg));
break;
}
case Observation::IIS:
{
reader.reset(new IisReader(arg));
break;
}
default:
{
NUKLEI_THROW("Unknown format.");
break;
}
}
reader->init();
return reader;
NUKLEI_TRACE_END();
}
/**
* This function implements the algorithm of Fig. 2: Simulated annealing
* algorithm of the ACCV paper.
* - T_j is given by @c temperature
* - u is given by Random::uniform()
* - w_j is @c current
*/
static void
metropolisHastings(const KernelCollection& objectEvidence,
const KernelCollection& sceneEvidence,
kernel::se3& current,
weight_t ¤tWeight,
const weight_t temperature,
const bool firstRun,
const int n)
{
NUKLEI_TRACE_BEGIN();
// Randomly select particles from the object model
std::vector<int> indices;
for (KernelCollection::const_sample_iterator
i = objectEvidence.sampleBegin(n);
i != i.end();
i++)
{
indices.push_back(i.index());
}
std::random_shuffle(indices.begin(), indices.end(), Random::uniformInt);
// Next chain state
kernel::se3 next;
// Whether we go for a local or independent proposal
bool independentProposal = false;
if (Random::uniform() < .75 || firstRun)
{
// Go for independent proposal
independentProposal = true;
kernel::se3::ptr k1 = objectEvidence.at(indices.front()).polySe3Proj();
kernel::se3::ptr k2 =
sceneEvidence.at(Random::uniformInt(sceneEvidence.size())).polySe3Proj();
next = k2->transformationFrom(*k1);
}
else
{
// Go for local proposal
independentProposal = false;
NUKLEI_DEBUG_ASSERT(current.loc_h_ > 0 && current.ori_h_ > 0);
next = current.sample();
}
weight_t weight = 0;
double threshold = Random::uniform();
// Go through the points of the model
for (unsigned pi = 0; pi < indices.size(); ++pi)
{
const kernel::base& objectPoint = objectEvidence.at(indices.at(pi));
kernel::base::ptr test = objectPoint.polyTransformedWith(next);
weight_t w = 0;
if (WEIGHTED_SUM_EVIDENCE_EVAL)
{
w = (sceneEvidence.evaluationAt(*test,
KernelCollection::WEIGHTED_SUM_EVAL) +
WHITE_NOISE_POWER/sceneEvidence.size() );
}
else
{
w = (sceneEvidence.evaluationAt(*test, KernelCollection::MAX_EVAL) +
WHITE_NOISE_POWER );
}
weight += w;
// At least consider sqrt(size(model)) points
if (pi < std::sqrt(indices.size())) continue;
weight_t nextWeight = weight/(pi+1);
// For the first run, consider all the points of the model
if (firstRun)
{
if (pi == indices.size()-1)
{
current = next;
currentWeight = nextWeight;
return;
}
else continue;
}
weight_t dec = std::pow(nextWeight/currentWeight, 1./temperature);
if (independentProposal) dec *= currentWeight/nextWeight;
// Early abort
if (dec < .6*threshold)
{
return;
}
// MH decision
if (pi == indices.size()-1)
{
if (dec > threshold)
{
current = next;
currentWeight = nextWeight;
}
return;
}
}
NUKLEI_THROW("Reached forbidden state.");
NUKLEI_TRACE_END();
}
