int DiscretePolicy::confSample(real* Qs, real* vQs) {
static NormalDistribution gaussian;
static LaplacianDistribution laplacian;
static UniformDistribution uniform;
for (int a=0; a<n_actions; a++) {
//eval[a] = Qs[a] + urandom(-1.0,1.0)*vQs[a];
switch(confidence_distribution) {
case SINGULAR:
sample[a] = Qs[a];
break;
case BOUNDED:
uniform.setMean(Qs[a]);
uniform.setVariance(vQs[a]);
sample[a] = uniform.generate();
break;
case GAUSSIAN:
gaussian.setMean(Qs[a]);
gaussian.setVariance(vQs[a]);
sample[a] = gaussian.generate();
break;
case LAPLACIAN:
laplacian.setMean(Qs[a]);
laplacian.setVariance(vQs[a]);
sample[a] = Qs[a] + laplacian.generate();
break;
default:
Serror ("Unknown distribution ID:%d\n", confidence_distribution);
break;
}
}
return argMax(sample);
}
TEST_F(AnalysisFixture, NormalDistribution) {
//Check default constructor
NormalDistribution defaultDistribution;
EXPECT_EQ(defaultDistribution.mean(),0.0);
EXPECT_EQ(defaultDistribution.standardDeviation(),1.0);
EXPECT_EQ(defaultDistribution.lowerBound(),boost::none);
//Check normal constructor
NormalDistribution distribution(2.0,3.0);
distribution.setLowerBound(0.0);
distribution.setUpperBound(4.0);
EXPECT_EQ(distribution.mean(),2.0);
EXPECT_EQ(distribution.standardDeviation(),3.0);
EXPECT_EQ(distribution.lowerBound(),0.0);
EXPECT_EQ(distribution.upperBound(),4.0);
}
TEST_F(AnalysisFixture,UncertaintyDescription_ConstructionCopyingAndCasting) {
NormalDistribution stdNormal;
{
UncertaintyDescription baseCopy = stdNormal; // copies impl
} // calls destructor
EXPECT_DOUBLE_EQ(0.0,stdNormal.mean());
EXPECT_DOUBLE_EQ(1.0,stdNormal.standardDeviation());
EXPECT_FALSE(stdNormal.lowerBound());
EXPECT_FALSE(stdNormal.upperBound());
{
UncertaintyDescription baseCopy(stdNormal);
NormalDistribution stdNormalCopy = baseCopy.cast<NormalDistribution>();
EXPECT_EQ(UncertaintyDescriptionType(UncertaintyDescriptionType::normal_uncertain),baseCopy.type());
}
{
GenericUncertaintyDescription genericCopy = stdNormal.cast<GenericUncertaintyDescription>();
EXPECT_EQ(2u,genericCopy.attributes().size());
}
}
ICPRegistrationTools::CC_ICP_RESULT ICPRegistrationTools::RegisterClouds(GenericIndexedCloudPersist* _modelCloud,
GenericIndexedCloudPersist* _dataCloud,
PointProjectionTools::Transformation& transform,
CC_ICP_CONVERGENCE_TYPE convType,
double minErrorDecrease,
unsigned nbMaxIterations,
double& finalError,
GenericProgressCallback* progressCb/*=0*/,
bool filterOutFarthestPoints/*=false*/,
unsigned samplingLimit/*=20000*/,
ScalarField* modelWeights/*=0*/,
ScalarField* dataWeights/*=0*/)
{
assert(_modelCloud && _dataCloud);
finalError = -1.0;
//MODEL CLOUD (reference, won't move)
GenericIndexedCloudPersist* modelCloud=_modelCloud;
ScalarField* _modelWeights=modelWeights;
{
if (_modelCloud->size()>samplingLimit) //shall we resample the clouds? (speed increase)
{
ReferenceCloud* subModelCloud = CloudSamplingTools::subsampleCloudRandomly(_modelCloud,samplingLimit);
if (subModelCloud && modelWeights)
{
_modelWeights = new ScalarField("ResampledModelWeights",modelWeights->isPositive());
unsigned realCount = subModelCloud->size();
if (_modelWeights->reserve(realCount))
{
for (unsigned i=0;i<realCount;++i)
_modelWeights->addElement(modelWeights->getValue(subModelCloud->getPointGlobalIndex(i)));
_modelWeights->computeMinAndMax();
}
else
{
//not enough memory
delete subModelCloud;
subModelCloud=0;
}
}
modelCloud = subModelCloud;
}
if (!modelCloud) //something bad happened
return ICP_ERROR_NOT_ENOUGH_MEMORY;
}
//DATA CLOUD (will move)
ReferenceCloud* dataCloud=0;
ScalarField* _dataWeights=dataWeights;
SimpleCloud* rotatedDataCloud=0; //temporary structure (rotated vertices)
{
if (_dataCloud->size()>samplingLimit) //shall we resample the clouds? (speed increase)
{
dataCloud = CloudSamplingTools::subsampleCloudRandomly(_dataCloud,samplingLimit);
if (dataCloud && dataWeights)
{
_dataWeights = new ScalarField("ResampledDataWeights",dataWeights->isPositive());
unsigned realCount = dataCloud->size();
if (_dataWeights->reserve(realCount))
{
for (unsigned i=0;i<realCount;++i)
_dataWeights->addElement(dataWeights->getValue(dataCloud->getPointGlobalIndex(i)));
_dataWeights->computeMinAndMax();
}
else
{
//not enough memory
delete dataCloud;
dataCloud=0;
}
}
}
else
{
//create a 'fake' reference cloud with all points
dataCloud = new ReferenceCloud(_dataCloud);
if (dataCloud->reserve(_dataCloud->size()))
{
dataCloud->addPointIndex(0,_dataCloud->size());
}
else //not enough memory
{
delete dataCloud;
dataCloud=0;
}
}
if (!dataCloud || !dataCloud->enableScalarField()) //something bad happened
{
if (dataCloud)
delete dataCloud;
if (modelCloud && modelCloud != _modelCloud)
delete modelCloud;
if (_modelWeights && _modelWeights!=modelWeights)
_modelWeights->release();
return ICP_ERROR_NOT_ENOUGH_MEMORY;
}
}
//Closest Point Set (see ICP algorithm)
ReferenceCloud* CPSet = new ReferenceCloud(modelCloud);
ScalarField* CPSetWeights = _modelWeights ? new ScalarField("CPSetWeights",_modelWeights->isPositive()) : 0;
//algorithm result
CC_ICP_RESULT result = ICP_NOTHING_TO_DO;
unsigned iteration = 0;
double error = 0.0;
//we compute the initial distance between the two clouds (and the CPSet by the way)
dataCloud->forEach(ScalarFieldTools::razDistsToHiddenValue);
DistanceComputationTools::Cloud2CloudDistanceComputationParams params;
params.CPSet = CPSet;
if (DistanceComputationTools::computeHausdorffDistance(dataCloud,modelCloud,params,progressCb)>=0)
{
//12/11/2008 - A.BEY: ICP guarantees only the decrease of the squared distances sum (not the distances sum)
error = DistanceComputationTools::computeMeanSquareDist(dataCloud);
}
else
{
//if an error occured during distances computation...
error = -1.0;
result = ICP_ERROR_DIST_COMPUTATION;
}
if (error > 0.0)
{
#ifdef _DEBUG
FILE* fp = fopen("registration_trace_log.txt","wt");
if (fp)
fprintf(fp,"Initial error: %f\n",error);
#endif
double lastError=error,initialErrorDelta=0.0,errorDelta=0.0;
result = ICP_APPLY_TRANSFO; //as soon as we do at least one iteration, we'll have to apply a transformation
while (true)
{
++iteration;
//regarding the progress bar
if (progressCb && iteration>1) //on the first iteration, we do... nothing
{
char buffer[256];
//then on the second iteration, we init/show it
if (iteration==2)
{
initialErrorDelta = errorDelta;
progressCb->reset();
progressCb->setMethodTitle("Clouds registration");
sprintf(buffer,"Initial mean square error = %f\n",lastError);
progressCb->setInfo(buffer);
progressCb->start();
}
else //and after we update it continuously
{
sprintf(buffer,"Mean square error = %f [%f]\n",error,-errorDelta);
progressCb->setInfo(buffer);
progressCb->update((float)((initialErrorDelta-errorDelta)/(initialErrorDelta-minErrorDecrease)*100.0));
}
if (progressCb->isCancelRequested())
break;
}
//shall we remove points with distance above a given threshold?
if (filterOutFarthestPoints)
{
NormalDistribution N;
N.computeParameters(dataCloud,false);
if (N.isValid())
{
DistanceType mu,sigma2;
N.getParameters(mu,sigma2);
ReferenceCloud* c = new ReferenceCloud(dataCloud->getAssociatedCloud());
ReferenceCloud* newCPSet = new ReferenceCloud(CPSet->getAssociatedCloud()); //we must also update the CPSet!
ScalarField* newdataWeights = (_dataWeights ? new ScalarField("ResampledDataWeights",_dataWeights->isPositive()) : 0);
//unsigned realCount = dataCloud->size();
//if (_dataWeights->reserve(realCount))
//{
// for (unsigned i=0;i<realCount;++i)
// _dataWeights->addElement(dataWeights->getValue(dataCloud->getPointGlobalIndex(i)));
// _dataWeights->computeMinAndMax();
//}
//else
//{
// //not enough memory
// delete dataCloud;
// dataCloud=0;
//}
unsigned n=dataCloud->size();
if (!c->reserve(n) || !newCPSet->reserve(n) || (newdataWeights && !newdataWeights->reserve(n)))
{
//not enough memory
delete c;
delete newCPSet;
if (newdataWeights)
newdataWeights->release();
result = ICP_ERROR_REGISTRATION_STEP;
break;
}
//we keep only the points with "not too high" distances
DistanceType maxDist = mu+3.0f*sqrt(sigma2);
unsigned realSize=0;
for (unsigned i=0;i<n;++i)
{
unsigned index = dataCloud->getPointGlobalIndex(i);
if (dataCloud->getAssociatedCloud()->getPointScalarValue(index)<maxDist)
{
c->addPointIndex(index);
newCPSet->addPointIndex(CPSet->getPointGlobalIndex(i));
if (newdataWeights)
newdataWeights->addElement(_dataWeights->getValue(index));
++realSize;
}
}
//resize should be ok as we have called reserve first
c->resize(realSize); //should always be ok as counter<n
newCPSet->resize(realSize); //idem
if (newdataWeights)
newdataWeights->resize(realSize); //idem
//replace old structures by new ones
delete CPSet;
CPSet=newCPSet;
delete dataCloud;
dataCloud=c;
if (_dataWeights)
{
_dataWeights->release();
_dataWeights = newdataWeights;
}
}
}
//update CPSet weights (if any)
if (_modelWeights)
{
unsigned count=CPSet->size();
assert(CPSetWeights);
if (CPSetWeights->currentSize()!=count)
{
if (!CPSetWeights->resize(count))
{
result = ICP_ERROR_REGISTRATION_STEP;
break;
}
}
for (unsigned i=0;i<count;++i)
CPSetWeights->setValue(i,_modelWeights->getValue(CPSet->getPointGlobalIndex(i)));
CPSetWeights->computeMinAndMax();
}
PointProjectionTools::Transformation currentTrans;
//if registration procedure fails
if (!RegistrationTools::RegistrationProcedure(dataCloud, CPSet, currentTrans, _dataWeights, _modelWeights))
{
result = ICP_ERROR_REGISTRATION_STEP;
break;
}
//get rotated data cloud
if (!rotatedDataCloud || filterOutFarthestPoints)
{
//we create a new structure, with rotated points
SimpleCloud* newDataCloud = PointProjectionTools::applyTransformation(dataCloud,currentTrans);
if (!newDataCloud)
{
//not enough memory
result = ICP_ERROR_REGISTRATION_STEP;
break;
}
//update dataCloud
if (rotatedDataCloud)
delete rotatedDataCloud;
rotatedDataCloud = newDataCloud;
delete dataCloud;
dataCloud = new ReferenceCloud(rotatedDataCloud);
if (!dataCloud->reserve(rotatedDataCloud->size()))
{
//not enough memory
result = ICP_ERROR_REGISTRATION_STEP;
break;
}
dataCloud->addPointIndex(0,rotatedDataCloud->size());
}
else
{
//we simply have to rotate the existing temporary cloud
rotatedDataCloud->applyTransformation(currentTrans);
}
//compute (new) distances to model
params.CPSet = CPSet;
if (DistanceComputationTools::computeHausdorffDistance(dataCloud,modelCloud,params)<0)
{
//an error occured during distances computation...
result = ICP_ERROR_REGISTRATION_STEP;
break;
}
lastError = error;
//12/11/2008 - A.BEY: ICP guarantees only the decrease of the squared distances sum (not the distances sum)
error = DistanceComputationTools::computeMeanSquareDist(dataCloud);
finalError = (error>0 ? sqrt(error) : error);
#ifdef _DEBUG
if (fp)
fprintf(fp,"Iteration #%i --> error: %f\n",iteration,error);
#endif
//error update
errorDelta = lastError-error;
//is it better?
if (errorDelta > 0.0)
{
//we update global transformation matrix
if (currentTrans.R.isValid())
{
if (transform.R.isValid())
transform.R = currentTrans.R * transform.R;
else
transform.R = currentTrans.R;
transform.T = currentTrans.R * transform.T;
}
transform.T += currentTrans.T;
}
//stop criterion
if ((errorDelta < 0.0) || //error increase
(convType == MAX_ERROR_CONVERGENCE && errorDelta < minErrorDecrease) || //convergence reached
(convType == MAX_ITER_CONVERGENCE && iteration > nbMaxIterations)) //max iteration reached
{
break;
}
}
if (progressCb)
progressCb->stop();
#ifdef _DEBUG
if (fp)
{
fclose(fp);
fp=0;
}
#endif
}
if (CPSet)
delete CPSet;
CPSet=0;
if (CPSetWeights)
CPSetWeights->release();
//release memory
if (modelCloud && modelCloud != _modelCloud)
delete modelCloud;
if (_modelWeights && _modelWeights!=modelWeights)
_modelWeights->release();
if (dataCloud)
delete dataCloud;
if (_dataWeights && _dataWeights!=dataWeights)
_dataWeights->release();
if (rotatedDataCloud)
delete rotatedDataCloud;
return result;
}
void TestNormalDistribution::testClone()
{
NormalDistribution distSigmaWithNumDeviations( *mean, *stdDev, numDevs );
NormalDistribution distSigmaWithoutNumDeviations( *mean, *stdDev );
NormalDistribution distBoundsWithoutNumDeviations( lb, ub );
NormalDistribution distBoundsWithNumDeviations( lb, ub, numDevs );
// call clone and test results
NormalDistribution* rtn = (NormalDistribution*)distSigmaWithNumDeviations.clone();
_test( rtn->lowerBound() == distSigmaWithNumDeviations.lowerBound() );
_test( rtn->upperBound() == distSigmaWithNumDeviations.upperBound() );
_test( rtn->mean() == distSigmaWithNumDeviations.mean() );
_test( rtn->stdDev() == distSigmaWithNumDeviations.stdDev() );
_test( rtn->typeName() == distSigmaWithNumDeviations.typeName() );
// clean up memory
delete rtn;
// call clone and test results
rtn = (NormalDistribution*)distSigmaWithoutNumDeviations.clone();
_test( rtn->lowerBound() == distSigmaWithoutNumDeviations.lowerBound() );
_test( rtn->upperBound() == distSigmaWithoutNumDeviations.upperBound() );
_test( rtn->mean() == distSigmaWithoutNumDeviations.mean() );
_test( rtn->stdDev() == distSigmaWithoutNumDeviations.stdDev() );
_test( rtn->typeName() == distSigmaWithoutNumDeviations.typeName() );
// clean up memory
delete rtn;
// call clone and test results
rtn = (NormalDistribution*)distBoundsWithoutNumDeviations.clone();
_test( rtn->lowerBound() == distBoundsWithoutNumDeviations.lowerBound() );
_test( rtn->upperBound() == distBoundsWithoutNumDeviations.upperBound() );
_test( rtn->mean() == distBoundsWithoutNumDeviations.mean() );
_test( rtn->stdDev() == distBoundsWithoutNumDeviations.stdDev() );
_test( rtn->typeName() == distBoundsWithoutNumDeviations.typeName() );
// clean up memory
delete rtn;
// call clone and test results
rtn = (NormalDistribution*)distBoundsWithNumDeviations.clone();
_test( rtn->lowerBound() ==distBoundsWithNumDeviations.lowerBound() );
_test( rtn->upperBound() ==distBoundsWithNumDeviations.upperBound() );
_test( rtn->mean() ==distBoundsWithNumDeviations.mean() );
_test( rtn->stdDev() ==distBoundsWithNumDeviations.stdDev() );
_test( rtn->typeName() ==distBoundsWithNumDeviations.typeName() );
// clean up memory
delete rtn;
}
double Pulsar::GaussianBaseline::get_variance_correction (double sigma)
{
NormalDistribution normal;
return 1.0 / normal.moment2 (-sigma, sigma);
}
int main ()
{
double a = 1.6;
NormalDistribution normal;
if (normal.density (a) <= normal.density (a+1)) {
fprintf (stderr, "probability is not decreasing\n");
return -1;
}
if (fabs(normal.density (a) - normal.density (-a)) > 1e-12) {
fprintf (stderr, "probability is not symmetric\n");
fprintf (stderr, "a=%lf; prob(a)=%lf; prob(-a)=%lf\n", a,
normal.density (a), normal.density (-a));
return -1;
}
if (normal.cumulative_distribution (a) >= normal.cumulative_distribution (a+1)) {
fprintf (stderr, "cummulative is not increasing\n");
return -1;
}
if (fabs(normal.cumulative_distribution (a) + normal.cumulative_distribution (-a) -1) > 1e-12) {
fprintf (stderr, "cummulative is not anti-symmetric about 0.5\n");
fprintf (stderr, "a=%lf; cummulative(a)=%lf; cummulative(-a)=%lf\n", a,
normal.cumulative_distribution (a), normal.cumulative_distribution (-a));
return -1;
}
if (normal.cumulative_distribution (0) != 0.5) {
fprintf (stderr, "cummulative(0) != 0.5\n");
return -1;
}
fprintf (stderr, "all normal distribution functions pass tests\n");
return 0;
}
