DirectionalLightSamplerCLProcessor::DirectionalLightSamplerCLProcessor()
: Processor(), KernelObserver()
, boundingVolume_("SceneGeometry")
, samplesPort_("samples")
, lightSamplesPort_("LightSamples")
, lights_("light")
, workGroupSize_("wgsize", "Work group size", 64, 1, 4096)
, useGLSharing_("glsharing", "Use OpenGL sharing", true)
, lightSamples_(std::make_shared<LightSamples>())
, lightSampler_(workGroupSize_.get())
{
addPort(boundingVolume_);
addPort(samplesPort_);
addPort(lights_);
addPort(lightSamplesPort_);
addProperty(workGroupSize_);
lights_.onChange([this]() {lightSamples_->resetIteration(); });
lightSampler_.setWorkGroupSize(workGroupSize_);
workGroupSize_.onChange([this]() { lightSampler_.setWorkGroupSize(workGroupSize_.get()); });
useGLSharing_.onChange([this]() { lightSampler_.setUseGLSharing(useGLSharing_); });
addObservation(&lightSampler_);
addObservation(&lightSampleMeshIntersector_);
}
void Hmm::baumWelch(vector<vector<unsigned long>*>& sequences, int iterations)
{
cerr << "Training with Baum-Welch for up to " << iterations << " iterations, using "
<< sequences.size() << " sequences." << endl;
double prevTotalLogProb = 0;
for (int k = 0; k<iterations; k++) {
PseudoCounts counts;
double totalLogProb = 0;
for (unsigned int i=0; i<sequences.size(); i++) {
vector<unsigned long>& seq = *sequences[i];
for (unsigned int j=0; j<seq.size(); j++) {
addObservation(seq[j]);
}
// accumulate the pseudo counts
totalLogProb += getPseudoCounts(counts);
reset();
if ((i+1)%1000==0)
cerr << "Processed " << i+1 << " sequences" << endl;
}
cerr << "Iteration " << k+1 << ' ' << "totalLogProb=" << totalLogProb << endl;
if (prevTotalLogProb!=0 && (totalLogProb - prevTotalLogProb<1))
break;
else
prevTotalLogProb = totalLogProb;
updateProbs(counts);
}
}
Observer& Observer::operator=(const Observer& that) {
if (this != &that) {
removeObservations();
for (auto observable : that.observables_) addObservation(observable);
}
return *this;
}
void Interpolate1D::setObservation(const float & x,
const float & y,
const int & idx)
{
if(idx >= numObservations() ) {
addObservation(x, y);
return;
}
m_observations[idx] = Float2(x, y);
}
void Execute::slotNext() {
switch( ui.stackedWidget->currentIndex() ) {
case 0: {
saveSession();
break;
}
case 1: {
addTargetNotes();
break;
}
case 2: {
addObservation();
ui.stackedWidget->setCurrentIndex( 1 );
ui.NextButton->setText( i18n( "Next Page >" ) );
QString prevTarget = currentTarget->name();
loadTargets();
ui.Target->setCurrentRow( findIndexOfTarget( prevTarget ), QItemSelectionModel::SelectCurrent );
selectNextTarget();
break;
}
}
}
bool Localization::Calculate(vector<LineSegment> &clusteredLines,
bool circleDetected, const Point2f &FieldHullRealCenter,
const vector<cv::Point2f> &FieldHullReal,
const Point2d &resultCircleRotated, const vector<Point2f> &goalPosition,
const bool &confiused, vector<LineContainer> &AllLines,
vector<FeatureContainer> &AllFeatures)
{
if (_cameraProjections == NULL)
{
ROS_ERROR("Error in programming");
return false;
}
AllLines.reserve(clusteredLines.size());
AllFeatures.reserve(5);
const double MAX_FASHER = 200.;
atLeastOneObservation = false;
double UPDATENORMAL = params.loc->UPDATENORMAL()
* params.loc->TOTALGAIN();
double UPDATESTRONG = params.loc->UPDATESTRONG()
* params.loc->TOTALGAIN();
double UPDATEWEAK = params.loc->UPDATEWEAK() * params.loc->TOTALGAIN();
LineSegment HorLine(cv::Point(0, -10), cv::Point(0, 10));
LineSegment VerLine(cv::Point(10, 0), cv::Point(-10, 0));
for (size_t i = 0; i < clusteredLines.size(); i++)
{
LineSegment lineSeg = clusteredLines[i];
if (lineSeg.GetLength() > params.loc->minLineLen())
{
cv::Point2d mid = lineSeg.GetMiddle();
if (lineSeg.GetAbsMinAngleDegree(VerLine) < 45)
{
LineType thisType = VerUndef;
double angleDiffVer = lineSeg.GetExteriorAngleDegree(VerLine);
if (angleDiffVer < -90)
angleDiffVer += 180;
if (angleDiffVer > 90)
angleDiffVer += -180;
if (lineSeg.DistanceFromLine(cv::Point(0, 0))
> params.loc->VerLineMinDistanceToUpdate())
{
LandmarkType ltype = CenterL;
double estimatedY = 0;
if (mid.y > 0)
{
thisType = VerLeft;
estimatedY = B2 - mid.y;
ltype = LeftL;
}
else
{
thisType = VerRight;
estimatedY = -B2 + abs(mid.y);
ltype = RightL;
}
addObservation(Point2d(0, estimatedY), 0,
MAX_FASHER * getUpdateCoef(UPDATENORMAL, lineSeg),
ltype);
}
else if (lineSeg.DistanceFromLine(FieldHullRealCenter)
> (params.loc->VerLineMinDistanceToUpdate() / 2.)
&& cv::contourArea(FieldHullReal) > 4)
{
LandmarkType ltype = CenterL;
LineSegment l2Test = lineSeg;
if (lineSeg.P1.x > lineSeg.P2.x)
{
l2Test.P1 = lineSeg.P2;
l2Test.P2 = lineSeg.P1;
}
double estimatedY = 0;
if (l2Test.GetSide(FieldHullRealCenter) < 0)
{
thisType = VerLeftNear;
estimatedY = B2 - mid.y;
ltype = LeftL;
}
else
{
thisType = VerRightNear;
estimatedY = -B2 + abs(mid.y);
ltype = RightL;
}
addObservation(Point2d(0, estimatedY), 0,
MAX_FASHER * getUpdateCoef(UPDATENORMAL, lineSeg),
ltype);
}
AllLines.push_back(LineContainer(lineSeg, thisType));
}
else
{
LineType thisType = HorUndef;
double angleDiffHor = lineSeg.GetExteriorAngleDegree(HorLine);
if (angleDiffHor < -90)
angleDiffHor += 180;
if (angleDiffHor > 90)
angleDiffHor += -180;
if (circleDetected
&& DistanceFromLineSegment(lineSeg, resultCircleRotated)
< 1)
{
thisType = HorCenter;
double estimatedX = -mid.x;
addObservation(Point2d(estimatedX, 0),
MAX_FASHER * UPDATENORMAL, 0, CenterL);
}
if (goalPosition.size() >= 2
&& lineSeg.DistanceFromLine(goalPosition[0]) < 0.5
&& lineSeg.DistanceFromLine(goalPosition[1]) < 0.5)
{
LandmarkType typel = CenterL;
double estimatedX = 0;
if (mid.x > 0)
{
typel = FrontL;
estimatedX = A2 - mid.x;
}
else
{
typel = BackL;
estimatedX = -A2 + abs(mid.x);
}
double lowPC = getUpdateCoef(
(goalPosition.size() == 2 ?
UPDATESTRONG : UPDATENORMAL), lineSeg);
addObservation(Point2d(estimatedX, 0), MAX_FASHER * lowPC,
0, typel);
thisType = HorGoal;
}
AllLines.push_back(LineContainer(lineSeg, thisType));
}
}
}
for (size_t i = 0; i < AllLines.size(); i++)
{
LineContainer hI = AllLines[i];
if (hI.type != HorUndef)
continue;
for (size_t j = i; j < AllLines.size(); j++)
{
LineContainer hJ = AllLines[j];
if (hJ.type != HorUndef)
continue;
cv::Point2d midI = hI.lineTransformed.GetMiddle();
cv::Point2d midJ = hJ.lineTransformed.GetMiddle();
double distanceToEachOther = dist3D_Segment_to_Segment(
hI.lineTransformed, hJ.lineTransformed);
double midPointsLSAngleToOne =
hI.lineTransformed.GetAbsMinAngleDegree(
LineSegment(midI, midJ));
if (distanceToEachOther < E * 1.5 && distanceToEachOther > E * 0.5
&& hI.lineTransformed.GetAbsMinAngleDegree(
hJ.lineTransformed) < 30
&& midPointsLSAngleToOne > 25)
{
bool hJ_Is_Goal_Line = hJ.lineTransformed.DistanceFromLine(
cv::Point(0, 0))
> hI.lineTransformed.DistanceFromLine(cv::Point(0, 0));
cv::Point2d mid = hJ_Is_Goal_Line ? midJ : midI;
double estimatedX = 0;
if ((hJ_Is_Goal_Line ? hJ.lineTransformed : hI.lineTransformed).DistanceFromLine(
cv::Point(0, 0)) > 1.2)
{
LandmarkType typel = CenterL;
if (mid.x > 0)
{
estimatedX = A2 - mid.x;
typel = FrontL;
}
else
{
estimatedX = -A2 + abs(mid.x);
typel = BackL;
}
double lowPC = getUpdateCoef(UPDATESTRONG,
hJ_Is_Goal_Line ?
hJ.lineTransformed : hI.lineTransformed);
addObservation(Point2d(estimatedX, 0), MAX_FASHER * lowPC,
0, typel);
}
}
}
}
if (circleDetected)
{
double estimatedX = -resultCircleRotated.x;
double estimatedY = -resultCircleRotated.y;
addObservation(Point2d(estimatedX, estimatedY),
MAX_FASHER * UPDATEWEAK, MAX_FASHER * UPDATEWEAK, CenterL);
}
if (atLeastOneObservation)
{
updateVertexIdx();
if ((nodeCounter % 30 == 0) && PreviousVertexId > 0)
{
optimizer.initializeOptimization();
optimizer.optimize(1);
Vector3d tmpV;
optimizer.vertex(PreviousVertexId)->getEstimateData(tmpV.data());
location.x = tmpV(0);
location.y = tmpV(1);
}
}
return true;
}
bool
cpu_tsdf::OctreeNode::addObservation (float d_new, float w_new, float max_weight,
uint8_t r, uint8_t g, uint8_t b)
{
return (addObservation (d_new, w_new, max_weight));
}
Observer::Observer(Observer&& rhs) {
for (auto observable : rhs.observables_) addObservation(observable);
rhs.removeObservations();
}
Observer::Observer(const Observer& rhs) {
for (auto observable : rhs.observables_) addObservation(observable);
}
