CrdObservation::CrdObservation()
{
NUKLEI_TRACE_BEGIN();
ColorDescriptor d;
k_.setDescriptor(d);
NUKLEI_TRACE_END();
}
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();
}
std::auto_ptr<KernelCollection> ObservationReader::readObservations()
{
NUKLEI_TRACE_BEGIN();
std::auto_ptr<KernelCollection> kcp(new KernelCollection);
readObservations(*kcp);
return kcp;
NUKLEI_TRACE_END();
}
void ObservationReader::addRegionOfInterest(boost::shared_ptr<RegionOfInterest> roi)
{
NUKLEI_TRACE_BEGIN();
if (roi_)
roi_->enqueue(roi);
else
roi_ = roi;
NUKLEI_TRACE_END();
}
KernelLogisticRegression::KernelLogisticRegression
(const KernelCollection &data,
const std::vector<int>& labels) :
trainSet_(data), labels_(labels)
{
NUKLEI_TRACE_BEGIN();
NUKLEI_ASSERT(trainSet_.size() == labels_.size());
computeGramMatrix();
NUKLEI_TRACE_END();
}
std::ostream& operator<<(std::ostream &out, const ObservationReader::Counter &c)
{
NUKLEI_TRACE_BEGIN();
typedef ObservationReader::Counter Counter;
for (Counter::list_t::const_iterator i = c.labels_.begin();
i != c.labels_.end(); ++i)
{
out << *i << ": " << c.counts_.find(*i)->second << "; ";
}
return out;
NUKLEI_TRACE_END();
}
void ObservationReader::Counter::incLabel(const std::string &label)
{
NUKLEI_TRACE_BEGIN();
map_t::iterator i;
if (counts_.count(label) == 0)
{
labels_.push_back(label);
i = counts_.insert(std::make_pair(label, unsigned(0))).first;
}
else i = counts_.find(label);
++(i->second);
NUKLEI_TRACE_END();
}
void ObservationReader::readObservations(KernelCollection &kc)
{
NUKLEI_TRACE_BEGIN();
kc.clear();
std::auto_ptr<Observation> o;
while ( (o = readObservation()).get() != NULL )
{
kc.add(*o->getKernel());
}
NUKLEI_TRACE_END();
}
void ObservationWriter::writeObservations(const KernelCollection &kc)
{
NUKLEI_TRACE_BEGIN();
std::auto_ptr<Observation> o = templateObservation();
for (KernelCollection::const_iterator i = kc.begin(); i != kc.end(); ++i)
{
o->setKernel(*i);
writeObservation(*o);
}
NUKLEI_TRACE_END();
}
void KernelLogisticRegression::computeGramMatrix()
{
NUKLEI_TRACE_BEGIN();
gramMatrix_ = GMatrix(trainSet_.size(), trainSet_.size());
for (unsigned i = 0; i < trainSet_.size(); ++i)
{
for (unsigned j = 0; j < trainSet_.size(); ++j)
{
gramMatrix_(i, j) = trainSet_.at(i).polyEval(trainSet_.at(j));
}
}
NUKLEI_TRACE_END();
}
std::auto_ptr<Observation> ObservationReader::readObservation()
{
NUKLEI_TRACE_BEGIN();
for (;;)
{
std::auto_ptr<Observation> observation = readObservation_();
if (observation.get() == NULL) return observation;
oc.incLabel("input");
if (roi_ && !roi_->contains(observation->getKernel()->getLoc()))
continue;
oc.incLabel("inROI");
return observation;
}
NUKLEI_TRACE_END();
}
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();
}
GMatrix KernelLogisticRegression::test(const KernelCollection &testSet) const
{
NUKLEI_TRACE_BEGIN();
GMatrix pr(2, testSet.size());
for (KernelCollection::const_iterator ti = testSet.begin();
ti != testSet.end(); ++ti)
{
Vector2 p = test(*ti);
pr(0,std::distance(testSet.begin(), ti)) = p.X();
pr(1,std::distance(testSet.begin(), ti)) = p.Y();
}
return pr;
NUKLEI_TRACE_END();
}
void KernelLogisticRegression::train(const double delta,
const unsigned itrNewton)
{
NUKLEI_TRACE_BEGIN();
NUKLEI_ASSERT(trainSet_.size() == labels_.size());
NUKLEI_ASSERT(trainSet_.size() != 0);
GMatrix vklr(trainSet_.size(), 2);
klr_train(gramMatrix_, gramMatrix_.GetRows(), gramMatrix_.GetColumns(),
&labels_.front(), labels_.size(),
vklr, vklr.GetRows(), vklr.GetColumns(),
delta, itrNewton);
vklr_ = vklr.Transpose();
NUKLEI_TRACE_END();
}
Vector2 KernelLogisticRegression::test(const kernel::base &t) const
{
NUKLEI_TRACE_BEGIN();
NUKLEI_ASSERT(vklr_);
Vector2 pr;
GMatrix Ktest(trainSet_.size(), 1);
for (unsigned i = 0; i < trainSet_.size(); ++i)
{
Ktest(i, 0) = trainSet_.at(i).polyEval(t);
}
GMatrix f = *vklr_ * Ktest;
double m = std::max(f(0,0), f(1,0));
double e1 = std::exp(f(0,0)-m), e2 = std::exp(f(1,0)-m);
pr.X() = e1/(e1+e2);
pr.Y() = e2/(e1+e2);
return pr;
NUKLEI_TRACE_END();
}
PCDObservation::PCDObservation()
{
NUKLEI_TRACE_BEGIN();
NUKLEI_TRACE_END();
}
NukleiObservation::NukleiObservation(const kernel::base& k)
{
NUKLEI_TRACE_BEGIN();
setKernel(k);
NUKLEI_TRACE_END();
}
NukleiObservation::NukleiObservation()
{
NUKLEI_TRACE_BEGIN();
NUKLEI_TRACE_END();
}
std::auto_ptr<ObservationReader>
ObservationReader::createReader(const std::string& arg)
{
NUKLEI_TRACE_BEGIN();
std::auto_ptr<ObservationReader> reader;
std::string errorsCat = std::string("Error in ObservationReader::createReader.") +
"\nErrors at each format attempt were:";
try {
reader = createReader(arg, Observation::COVIS3D);
return reader;
} catch (ObservationIOError &e) {
errorsCat += "\n" + std::string(e.what());
}
try {
reader = createReader(arg, Observation::NUKLEI);
return reader;
} catch (ObservationIOError &e) {
errorsCat += "\n" + std::string(e.what());
}
try {
reader = createReader(arg, Observation::IIS);
return reader;
} catch (ObservationIOError &e) {
errorsCat += "\n" + std::string(e.what());
}
try {
reader = createReader(arg, Observation::OSUTXT);
return reader;
} catch (ObservationIOError &e) {
errorsCat += "\n" + std::string(e.what());
}
try {
reader = createReader(arg, Observation::PCD);
return reader;
} catch (ObservationIOError &e) {
errorsCat += "\n" + std::string(e.what());
}
try {
reader = createReader(arg, Observation::PLY);
return reader;
} catch (ObservationIOError &e) {
errorsCat += "\n" + std::string(e.what());
}
try {
reader = createReader(arg, Observation::RIF);
return reader;
} catch (ObservationIOError &e) {
errorsCat += "\n" + std::string(e.what());
}
try {
reader = createReader(arg, Observation::SERIAL);
return reader;
} catch (ObservationIOError &e) {
errorsCat += "\n" + std::string(e.what());
}
try {
reader = createReader(arg, Observation::CRD);
return reader;
} catch (ObservationIOError &e) {
errorsCat += "\n" + std::string(e.what());
}
try {
reader = createReader(arg, Observation::OFF);
return reader;
} catch (ObservationIOError &e) {
errorsCat += "\n" + std::string(e.what());
}
try {
reader = createReader(arg, Observation::BUILTINVTK);
return reader;
} catch (ObservationIOError &e) {
errorsCat += "\n" + std::string(e.what());
}
try {
reader = createReader(arg, Observation::TXT);
return reader;
} catch (ObservationIOError &e) {
errorsCat += "\n" + std::string(e.what());
}
throw ObservationIOError
("Error loading observations with automatic type detection. "
"Maybe the filename `" + arg + "' is incorrect. "
"Else please try again with a defined type.");
return reader;
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();
}
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();
}
void RIFReader::reset()
{
NUKLEI_TRACE_BEGIN();
init();
NUKLEI_TRACE_END();
}
bool ObservationReader::Counter::empty() const
{
NUKLEI_TRACE_BEGIN();
return labels_.empty();
NUKLEI_TRACE_END();
}
const Color& CrdObservation::getColor() const
{
NUKLEI_TRACE_BEGIN();
return dynamic_cast<const ColorDescriptor&>(k_.getDescriptor()).getColor();
NUKLEI_TRACE_END();
}
void CrdObservation::setLoc(Vector3 loc)
{
NUKLEI_TRACE_BEGIN();
k_.loc_ = loc;
NUKLEI_TRACE_END();
}
void CrdObservation::setWeight(weight_t weight)
{
NUKLEI_TRACE_BEGIN();
k_.setWeight(weight);
NUKLEI_TRACE_END();
}
void OsuTxtReader::reset()
{
NUKLEI_TRACE_BEGIN();
init();
NUKLEI_TRACE_END();
}
void CrdObservation::setColor(const Color& color)
{
NUKLEI_TRACE_BEGIN();
dynamic_cast<ColorDescriptor&>(k_.getDescriptor()).setColor(color);
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();
}
