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(); }