void addNoiseTrack(struct noiseTrack *track, DNA *chunk, int chunkSize, struct cdnaAli *ca)
/* Add in contents of one cdna to noise tracks. */
{
struct ffAli *left, *right;
for (left = ca->ali; left != NULL; left = left->right)
{
DNA *h = left->hStart;
DNA *n = left->nStart;
int size = left->nEnd - n;
int hcOff = h-chunk;
int i;
for (i=0; i<size; ++i)
{
int noiseIx = hcOff + i;
if (noiseIx >= 0 && noiseIx < chunkSize)
{
track[noiseIx].cdnaCount += 1;
if (h[i] != n[i])
{
addNoise(&track[noiseIx], ca, n[i]);
}
}
}
right = left->right;
if (right != NULL)
{
int hGap = right->hStart - left->hEnd;
int nGap = right->nStart - left->nEnd;
if (hGap <= 3 && nGap <= 3 && hGap >= 0 && nGap >= 0)
/* Treat small gaps as noise. */
{
if (hGap == nGap || nGap == 0)
{
int i;
for (i=0; i<nGap; ++i)
{
int noiseIx = left->hEnd - chunk + i;
if (noiseIx >= 0 && noiseIx < chunkSize)
{
char noiseC = (nGap == 0 ? 'i' : left->nEnd[i]);
addNoise(&track[noiseIx], ca, noiseC);
track[noiseIx].cdnaCount += 1;
}
}
}
}
}
}
}
/*
* When the robot moves.
* add to each particle the diff according to it's angle.
* the Diff diff is not aware of the robots angle, it just knows the distance it moved relatively
* TODO I think we don't need to add the diff to all the particles all the time,
* just sum the diffs in 1 particle, and when pole data arrives, add all the particles the diff
* we summed in that 1 particle. then reset that particle to [0,0,0]
* it will save CPU usage, but need to think if its right to do it, mathematically.
*/
void Algorithm::ParticleFilter(Diff& diff)
{
if (diff.angle==0 && diff.x==0 && diff.y==0)
return;
if (++m_stepsWithoutPolePicture > 20)
m_outOfLocation = true;
Angle ang = DEG2RAD*m_bestLocation.angle;
//update the best particles
m_bestLocation.time=diff.time;
m_bestLocation.angle+=diff.angle/2; //walking direction to the mean angle
m_bestLocation.y+=diff.y*cos(ang);
m_bestLocation.y+=diff.x*sin(ang);
m_bestLocation.x+=diff.y*sin(ang);
m_bestLocation.x+=diff.x*cos(ang);
m_bestLocation.angle+=diff.angle/2;
//update all the particles
for (State_iterator it = m_currState.begin(); it != m_currState.end() ;it++)
{
ang = DEG2RAD*it->angle;
it->angle+=diff.angle/2;
it->y+=diff.y*cos(ang);
it->y+=diff.x*sin(ang);
it->x+=diff.y*sin(ang);
it->x+=diff.x*cos(ang);
it->angle+=diff.angle/2;
}
addNoise(0.5);
}
// This function generates the whole signal, it is not limited to the needed samples
void ExperimentInput::process()
{
chrono->start("Input");
for (int k = 0; k < config->getSignalLength(); k++)
{
neededSamples.insert(k);
}
generateNonZeroFrequencies();
frequencyToTime();
if (config->isNoisy())
{
addNoise();
}
// scale the Fourier transform
for (auto t = neededSamples.cbegin(); t != neededSamples.cend(); ++t)
{
timeSignal[*t] /= sqrt((ffast_complex)config->getSignalLengthOriginal());
}
if (config->isQuantized())
{
applyQuantization(config->getQuantizationBitsNb());
}
chrono->stop("Input");
}
void ExperimentInput::process(std::vector<int> delays)
{
chrono->start("Input");
findNeededSamples(delays);
generateNonZeroFrequencies();
// if the problem is not off-grid, generate the signal using FFT
if ( config->getSignalLengthOriginal() != config->getSignalLength() )
{
frequencyToTime();
}
else
{
frequencyToTimeUsingFFT(delays);
}
if (config->isNoisy())
{
addNoise();
}
for (auto t = neededSamples.cbegin(); t != neededSamples.cend(); ++t)
{
timeSignal[*t] /= sqrt((ffast_complex)config->getSignalLengthOriginal());
}
chrono->stop("Input");
}
void MainWindow::on_add_noise_button_clicked()
{
noiseLevel = ui->noise_level_box->value();
addNoise();
renderNoisy();
ui->tabWidget->setCurrentIndex(1);
}
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
// Default setting
//srand(time(NULL));
gamma = 1.0;
nbeta = 10;
noiseLevel = 0.2;
ui->setupUi(this);
// Set default manual run instead of auto run
ui->auto_run_check->setChecked(true);
autorun = true;
ui->nbeta_box->setText(QString::number(nbeta));
ui->gamma_box->setText(QString::number(gamma));
ui->gamma_slider->setRange(0, 300);
ui->gamma_slider->setValue(gamma * 100.0);
ui->noise_level_box->setRange(0.01, 0.4);
ui->noise_level_box->setSingleStep(0.01);
ui->noise_level_box->setValue(noiseLevel);
ui->ratio_slider->setRange(0, 255);
ui->ratio_slider->blockSignals(true);
ui->ratio_slider->setValue(90);
ui->ratio_slider->blockSignals(false);
ui->ratio_display->setText(QString::number(120 / (double) 255));
loadImage(QString("./lena.bmp"));
renderOriginal();
addNoise();
renderNoisy();
}
void MainWindow::on_image_path_clicked()
{
QString file = QFileDialog::getOpenFileName(this, tr("Select an image data file"), "./", tr("Image files (*.png *.PNG *.jpg *.JPG *.jpeg *.JPEG *.bmp)"));
ui->path_display->setText(file.split("/").back());
loadImage(file);
renderOriginal();
ui->tabWidget->setCurrentIndex(0);
addNoise();
renderNoisy();
}
void MainWindow::on_ratio_slider_valueChanged(int value)
{
if(!binary){
int ratio = ui->ratio_slider->value();
ui->ratio_display->setText(QString::number(ratio / (double) 255));
gray2bin(ratio);
renderOriginal();
addNoise();
renderNoisy();
}
}
// This gets called by the world update start event.
void GazeboImuPlugin::OnUpdate(const common::UpdateInfo& _info) {
common::Time current_time = world_->GetSimTime();
double dt = (current_time - last_time_).Double();
last_time_ = current_time;
double t = current_time.Double();
math::Pose T_W_I = link_->GetWorldPose(); //TODO(burrimi): Check tf.
math::Quaternion C_W_I = T_W_I.rot;
math::Vector3 velocity_current_W = link_->GetWorldLinearVel();
// link_->GetRelativeLinearAccel() does not work sometimes. Returns only 0.
// TODO For an accurate simulation, this might have to be fixed. Consider the
// time delay introduced by this numerical derivative, for example.
math::Vector3 acceleration = (velocity_current_W - velocity_prev_W_) / dt;
math::Vector3 acceleration_I =
C_W_I.RotateVectorReverse(acceleration - gravity_W_);
math::Vector3 angular_vel_I = link_->GetRelativeAngularVel();
Eigen::Vector3d linear_acceleration_I(acceleration_I.x,
acceleration_I.y,
acceleration_I.z);
Eigen::Vector3d angular_velocity_I(angular_vel_I.x,
angular_vel_I.y,
angular_vel_I.z);
addNoise(&linear_acceleration_I, &angular_velocity_I, dt);
// Fill IMU message.
imu_message_.header.stamp.sec = current_time.sec;
imu_message_.header.stamp.nsec = current_time.nsec;
// TODO(burrimi): Add orientation estimator.
imu_message_.orientation.w = 1;
imu_message_.orientation.x = 0;
imu_message_.orientation.y = 0;
imu_message_.orientation.z = 0;
// imu_message_.orientation.w = C_W_I.w;
// imu_message_.orientation.x = C_W_I.x;
// imu_message_.orientation.y = C_W_I.y;
// imu_message_.orientation.z = C_W_I.z;
imu_message_.linear_acceleration.x = linear_acceleration_I[0];
imu_message_.linear_acceleration.y = linear_acceleration_I[1];
imu_message_.linear_acceleration.z = linear_acceleration_I[2];
imu_message_.angular_velocity.x = angular_velocity_I[0];
imu_message_.angular_velocity.y = angular_velocity_I[1];
imu_message_.angular_velocity.z = angular_velocity_I[2];
imu_pub_.publish(imu_message_);
velocity_prev_W_ = velocity_current_W;
}
ParticleSystem::ParticleSystem()
: m_numActive(0)
{
const int32_t N = GRID_RESOLUTION;
m_count = N * N * N;
m_pos = new nv::vec3f [m_count];
m_zs = new float [m_count];
m_sortedIndices16 = new GLushort [m_count];
initGrid(N);
addNoise(1.9, 1.0);
}
void rgb_noise(f0r_instance_t instance, double time,
const uint32_t* inframe, uint32_t* outframe)
{
rgbnoise_instance_t* inst = (rgbnoise_instance_t*)instance;
unsigned int len = inst->width * inst->height;
unsigned char* dst = (unsigned char*)outframe;
const unsigned char* src = (unsigned char*)inframe;
int sample;
double noise = inst->noise;
while (len--)
{
sample = *src++;
*dst++ = addNoise(sample, noise);
sample = *src++;
*dst++ = addNoise(sample, noise);
sample = *src++;
*dst++ = addNoise(sample, noise);
*src++;
*dst++;
}
}
void Algorithm::UpdateStates()
{
State prevState = m_currState;
double meanWeight = 1.0/prevState.size();
//how much noise to add: if we are in good location, don't add much noise,
//so it won't throw us too far, its important for stability.
//but if we are in bad location, add more noise, so
//we will converge to the real location faster.
int howMuchNoise = IN_FIELD_NOISE;
if (m_outOfLocation)
howMuchNoise = OUT_FIELD_NOISE;
unsigned n = 0;
double randWeight = drand48()*meanWeight;
double summedWeight = 0;
//choose new set of particles with Low variance sampler
for (State_iterator it_prev = prevState.begin(); it_prev != prevState.end() ;it_prev++)
{
summedWeight += it_prev->weight;
while (summedWeight > randWeight)
{
//choose the same particle as long its weight is larger than summedWeight
if(n <= m_currState.size())
m_currState[n++] = *it_prev;
else {
cout << "[Algorithm::UpdateStates] Critical segmentation error!" << endl;
break;
}
randWeight += meanWeight;
}
}
//This "if" should NEVER happen
if(n==0)
{
cout << "[Algorithm::UpdateStates] Critical localization error!" << endl;
for (State_iterator it = m_currState.begin(); it != m_currState.end() ;it++)
{
it->randomize();
}
} else {
UpdateBestLocation(); //weighted mean of all particles
addNoise(howMuchNoise);
}
printHistogram();
}
float CInfiniteAmortizedNoise3D::generate(int x, int y, int z, const int m0, const int m1, int n, float*** cell){
int r = 1; // Side length of cell divided by side length of subcell.
//skip over unwanted octaves
for(int i=1; i<m0; i++){
n /= 2; r *= 2;
} //for
if(n < 2)return 1.0f; //fail and bail - should not happen
//Generate first octave directly into cell.
// We could add all octaves into the cell directly if we zero out the cell
// before we begin. However, that is a nontrivial overhead that Perlin noise
// does not have, and we can avoid it too by putting in the first octave and
// adding in the rest.
initSplineTable(n); //initialize the spline table to cells of size n
for(int i0=0; i0<r; i0++)
for(int j0=0; j0<r; j0++)
for(int k0=0; k0<r; k0++){ //for each subcell
initEdgeTables(x + i0, y + j0, z + k0, n); //initialize the amortized noise tables
getNoise(n, i0*n, j0*n, k0*n, cell); //generate noise directly into cell
} //for
float scale = 1.0f; //scale factor
//Generate the other octaves and add them into cell. See previous comment.
for(int k=m0; k<m1 && n>=2; k++){ //for each octave after the first
n /= 2; r += r; x += x; y += y; z += z; scale *= 0.5f; //rescale for next octave
initSplineTable(n); //initialize the spline table to cells of size n
for(int i0=0; i0<r; i0++)
for(int j0=0; j0<r; j0++)
for(int k0=0; k0<r; k0++){ //for each subcell
initEdgeTables(x + i0, y + j0, z + k0, n); //initialize the edge tables
addNoise(n, i0*n, j0*n, k0*n, scale, cell); //generate directly into cell
} //for
} //for each octave
//Compute 1/magnitude and return it.
// A single octave of Perlin noise returns a value of magnitude at most
// 1/sqrt(3). Adding magnitudes over all scaled octaves gives a total
// magnitude of (1 + 0.5 + 0.25 +...+ scale)/sqrt(3). This is equal to
// (2 - scale)/sqrt(3) (using the standard formula for the sum of a geometric
// progression). 1/magnitude is therefore sqrt(3)/(2-scale).
return 1.732f/(2.0f - scale); //scale factor
} //generate
void makeMap(peak_param *peak, gal_map *gMap, map_t *kMap, map_t *nMap, FFT_t *transformer, error **err)
{
//-- Map making main function
if (peak->doKappa == 1) {
//-- Do convergence
kappaMapFromKappa(gMap, kMap, err); forwardError(*err, __LINE__,);
subtractMean_map_t(peak, kMap);
if (peak->doNewNoise == 1) {
//-- Overwrite nMap
u_int64_t seed = renewSeed();
fillNoise(peak, nMap, seed);
}
addNoise(kMap, nMap, err); forwardError(*err, __LINE__,);
doFFTSmoothing(peak, kMap, transformer, err); forwardError(*err, __LINE__,);
}
ParticleInitializer::ParticleInitializer():
m_pos(NULL),
m_zs(NULL),
m_sortedIndices16(NULL),
m_numActive(0),
m_width(480),
m_elapsedTime(0.0f)
{
const int32_t N = GRID_RESOLUTION;
m_count = N * N;
m_pos = new vec4f [m_count];
m_zs = new float [m_count];
m_sortedIndices16 = new GLushort [m_count];
initGrid(GRID_RESOLUTION);
addNoise(0.01, 70.0);
}
void test8uc3(bool show=false)
{
Mat src = imread("image.png");
Mat noise;
Mat dest;
int64 pre;
float sigma = 15;
cout<<"color"<<endl;
cout<<"sigma: "<<sigma<<endl;
cout<<"RAW: "<<endl;
addNoise(src,noise,sigma);
cout<<PSNR(src,noise)<<"dB"<<endl;
cout<<endl;
cout<<"NML opencv: "<<endl;
pre = getTickCount();
fastNlMeansDenoisingColored(noise,dest,sigma,sigma,3,7);
cout<<(getTickCount()-pre)/(getTickFrequency())*1000<<"ms"<<endl;
cout<<PSNR(src,dest)<<"dB"<<endl;
cout<<endl;
cout<<"NML base: "<<endl;
pre = getTickCount();
nonLocalMeansFilterBase(noise,dest,3,7,2*sigma);
cout<<(getTickCount()-pre)/(getTickFrequency())*1000<<"ms"<<endl;
cout<<PSNR(src,dest)<<"dB"<<endl;
cout<<endl;
cout<<"NML sse4: "<<endl;
pre = getTickCount();
nonLocalMeansFilter(noise,dest,3,7,2*sigma);
cout<<(getTickCount()-pre)/(getTickFrequency())*1000<<"ms"<<endl;
cout<<PSNR(src,dest)<<"dB"<<endl;
cout<<endl;
if(show)
{
imshow("noise",noise);
imshow("NLM",dest);
waitKey();
}
}
void updateOutput()
{
Output output;
const GroundTruth& ground_truth = getGroundTruth();
output.angular_velocity = ground_truth.kinematics->twist.angular;
output.linear_acceleration = ground_truth.kinematics->accelerations.linear - ground_truth.environment->getState().gravity;
output.orientation = ground_truth.kinematics->pose.orientation;
//acceleration is in world frame so transform to body frame
output.linear_acceleration = VectorMath::transformToBodyFrame(output.linear_acceleration,
ground_truth.kinematics->pose.orientation, true);
//add noise
addNoise(output.linear_acceleration, output.angular_velocity);
// TODO: Add noise in orientation?
setOutput(output);
}
void test32fc3(bool show=false)
{
Mat src_ = imread("image.png");
Mat noise_;
Mat dest;
int64 pre;
float sigma = 15;
cout<<"color"<<endl;
cout<<"sigma: "<<sigma<<endl;
cout<<"RAW: "<<endl;
addNoise(src_,noise_,sigma);
Mat src,noise;
src_.convertTo(src,CV_32F);
noise_.convertTo(noise,CV_32F);
cout<<PSNR_32f28u(src,noise)<<"dB"<<endl;
cout<<endl;
cout<<"NML base: "<<endl;
pre = getTickCount();
nonLocalMeansFilterBase(noise,dest,3,7,2*sigma);
cout<<(getTickCount()-pre)/(getTickFrequency())*1000<<"ms"<<endl;
cout<<PSNR_32f28u(src,dest)<<"dB"<<endl;
cout<<endl;
cout<<"NML sse4: "<<endl;
pre = getTickCount();
nonLocalMeansFilter(noise,dest,3,7,2*sigma);
cout<<(getTickCount()-pre)/(getTickFrequency())*1000<<"ms"<<endl;
cout<<PSNR_32f28u(src,dest)<<"dB"<<endl;
cout<<endl;
if(show)
{
imshow("noise",noise);
imshow("NLM",dest);
waitKey();
}
}
void HexLatticeGenerator::setAgentPosition(size_t i, BaseAgent* agt) {
if (i >= _totalPop) {
throw AgentGeneratorFatalException(
"HexLatticeGenerator trying to access an agent "
"outside of the specified population");
}
// Compute local position
const float R = _nbrDist * 0.5f;
float x, y;
if (_rowDir == ROW_X) {
const size_t BAND_POP = _rowPop * 2 - 1;
size_t band = i / BAND_POP; // number of full preceeding bands
i -= band * BAND_POP;
if (i >= _rowPop) {
// minor row
i -= _rowPop;
x = R + i * _nbrDist;
y = band * 2.f * _rowDist + _rowDist;
} else {
// major row
x = i * _nbrDist;
y = band * 2.f * _rowDist;
}
} else {
const size_t column = i / _rowPop; // number of full columns preceding i
i -= _rowPop * column;
x = column * _rowDist;
y = i * _nbrDist;
if (column % 2) {
y += R;
}
}
Vector2 p = addNoise(Vector2(x, y));
// rotated
Vector2 r = Vector2(_cosRot * p._x - _sinRot * p._y, _cosRot * p._y + _sinRot * p._x);
// world
agt->_pos = _anchor + r;
}
/**
* The synthetic video sequence we will work with here is composed of a
* single moving object, circular in shape (fixed radius)
* The motion here is a linear motion
* the foreground intensity and the backgrounf intensity is known
* the image is corrupted with zero mean Gaussian noise
* @param I The video itself
* @param IszX The x dimension of the video
* @param IszY The y dimension of the video
* @param Nfr The number of frames of the video
* @param seed The seed array used for number generation
*/
void videoSequence(int * I, int IszX, int IszY, int Nfr, int * seed){
int k;
int max_size = IszX*IszY*Nfr;
/*get object centers*/
int x0 = (int)roundDouble(IszY/2.0);
int y0 = (int)roundDouble(IszX/2.0);
I[x0 *IszY *Nfr + y0 * Nfr + 0] = 1;
/*move point*/
int xk, yk, pos;
for(k = 1; k < Nfr; k++){
xk = abs(x0 + (k-1));
yk = abs(y0 - 2*(k-1));
pos = yk * IszY * Nfr + xk *Nfr + k;
if(pos >= max_size)
pos = 0;
I[pos] = 1;
}
/*dilate matrix*/
int * newMatrix = (int *)malloc(sizeof(int)*IszX*IszY*Nfr);
memset(newMatrix, 0, sizeof(int) * max_size);
imdilate_disk(I, IszX, IszY, Nfr, 5, newMatrix);
int x, y;
for(x = 0; x < IszX; x++){
for(y = 0; y < IszY; y++){
for(k = 0; k < Nfr; k++){
I[x*IszY*Nfr + y*Nfr + k] = newMatrix[x*IszY*Nfr + y*Nfr + k];
}
}
}
free(newMatrix);
/*define background, add noise*/
setIf(0, 100, I, &IszX, &IszY, &Nfr);
setIf(1, 228, I, &IszX, &IszY, &Nfr);
/*add noise*/
addNoise(I, &IszX, &IszY, &Nfr, seed);
}
void SensorPointXY::sense() {
_robotPoseObject=0;
RobotType* r= dynamic_cast<RobotType*>(robot());
std::list<PoseObject*>::reverse_iterator it=r->trajectory().rbegin();
int count = 0;
while (it!=r->trajectory().rend() && count < 1){
if (!_robotPoseObject)
_robotPoseObject = *it;
it++;
count++;
}
for (std::set<BaseWorldObject*>::iterator it=world()->objects().begin();
it!=world()->objects().end(); it++){
WorldObjectType* o=dynamic_cast<WorldObjectType*>(*it);
if (o && isVisible(o)){
EdgeType* e=mkEdge(o);
if (e && graph()) {
e->setMeasurementFromState();
addNoise(e);
graph()->addEdge(e);
}
}
}
}
void MultilayerPerceptron::run()
{
vector<vector<double> >
inputs = ts->getInputs(),
targets = ts->getTargets();
StopCondition
//BP parameters
sc = (StopCondition)mlpbpts->getStopParameter();
double
//SA parameters
startCondition = 0,
To = 0,
minNoise = 0,
maxNoise = 0,
tempDecFactor = 0,
Tmin = 0,
//BP parameters
learningRate = mlpbpts->getLearningRate(),
MSEmin = mlpbpts->getMinMSE(),
RMSEmin = mlpbpts->getMinRMSE(),
CEmin = mlpbpts->getMinCE();
unsigned int
//SA parameters
nChanges = 0,
//BP parameters
epochs = mlpbpts->getMaxEpochs();
if(sa){
startCondition = mlpsats->getLocalMinimumCondition();
To = mlpsats->getTo();
minNoise = mlpsats->getMinNoise();
maxNoise = mlpsats->getMaxNoise();
tempDecFactor = mlpsats->getTempDecrementFactor();
Tmin = mlpsats->getMinTemperature();
nChanges = mlpsats->getChanges();
}
size_t
nPatterns,
nNeurons,
nBOutputs,
nOutputs;
vector<double>
yObtained,
deltaOut(outputWeights.size(), 0);
// vector<vector<double> > deltaHidden(layerWeights.size());
deltaHidden.resize(layerWeights.size());
for(size_t i = 0; i < deltaHidden.size(); i++){
size_t sLayer = layerWeights[i].size();
deltaHidden[i].resize(sLayer, 0);
}
nPatterns = inputs.size();
int nLayers = int(layerWeights.size());
double pMSE;
// unsigned long epc;
double sumDeltas;
nOutputs = getOutputSize();
// MultilayerPerceptron::TrainingResult tr;
tres->time = 0;
tres->epochs = 0;
tres->MSEHistory.clear();
tres->MSE = getMSE(inputs, targets);
tres->MSEHistory.push_back(tres->MSE);
tres->RMSEHistory.clear();
tres->RMSE = getRMSE(inputs, targets);
tres->RMSEHistory.push_back(tres->RMSE);
tres->CEHistory.clear();
tres->CE = getCE(inputs, targets);
tres->CEHistory.push_back(tres->CE);
// tres.layerWeightsHistory.clear();
// tres.layerWeightsHistory.push_back(layerWeights);
// tres.outputWeightsHistory.clear();
// tres.outputWeightsHistory.push_back(outputWeights);
vector<vector<double> > layerOutputs;
long double
T = 0,
sumDeltaF = 0,
deltaF = 0,
Pa = 0,
avgDeltaF = 0;
int c = 0;
training = true;
clock_t t_ini = clock();
do{
// tr.MSE = 0;
// pMSE = 0;
for(size_t p = 0; p < nPatterns; p++){
//Se obtienen las salidas para cada una de las capas
layerOutputs = getLayerOutputs(inputs[p]);
yObtained = layerOutputs[layerOutputs.size() - 1];
for(int layer = nLayers; layer >= 0; layer--){
nNeurons = (layer == nLayers ? outputWeights.size() : layerWeights[layer].size());
// deltaOut = vector<double>(nNeurons, 0);
for(size_t neuron = 0; neuron <= nNeurons; neuron++){
//Se inicia el calculo de todos los deltas
if(layer == nLayers){ //Si es la capa de salida
if(neuron < nNeurons){
switch(tf){
case Sigmoid:
deltaOut[neuron] = alfa * yObtained[neuron] * (1 - yObtained[neuron]) * (targets[p][neuron] - yObtained[neuron]);
break;
case Tanh:
deltaOut[neuron] = alfa * (1 - (yObtained[neuron]*yObtained[neuron])) * (targets[p][neuron] - yObtained[neuron]);
break;
}
}else{
continue;
}
}else{
size_t nDeltaElements = (layer == nLayers - 1 ? outputWeights.size() : layerWeights[layer + 1].size());
sumDeltas = 0;
for(size_t element = 0; element < nDeltaElements; element++){
if(layer == nLayers - 1){
sumDeltas += deltaOut[element] * outputWeights[element][neuron];
}else{
sumDeltas += deltaHidden[layer+1][element] * layerWeights[layer+1][element][neuron];
}
}
switch(tf){
case Sigmoid:
deltaHidden[layer][neuron] = alfa * layerOutputs[layer][neuron] * (1 - layerOutputs[layer][neuron]) * sumDeltas;
break;
case Tanh:
deltaHidden[layer][neuron] = alfa * (1 - (layerOutputs[layer][neuron]*layerOutputs[layer][neuron])) * sumDeltas;
break;
}
}
}
}
//Comienza la actualizacion de los pesos
for(int layer = nLayers; layer >= 0; layer--){
nNeurons = (layer == nLayers ? nOutputs : layerWeights[layer].size());
for(size_t i = 0; i < nNeurons; i++){
nBOutputs = (layer == 0 ? inputs[p].size() : layerWeights[layer - 1].size());
for(size_t j = 0; j <= nBOutputs; j++){
if(layer == nLayers){
outputWeights[i][j] += (j == nBOutputs ? -learningRate*deltaOut[i] : learningRate*deltaOut[i]*layerOutputs[layer-1][j]);
}else if(layer == 0){
layerWeights[layer][i][j] += (j == nBOutputs ?
-learningRate*deltaHidden[layer][i] :
learningRate*deltaHidden[layer][i]*inputs[p][j]);
}else{
layerWeights[layer][i][j] += (j == nBOutputs ? -learningRate*deltaHidden[layer][i] : learningRate*deltaHidden[layer][i]*layerOutputs[layer-1][j]);
}
}
}
}
}
pMSE = getMSE(inputs, targets);
if(sa){//if Simulated annealing activated
deltaF = pMSE - tres->MSE;
sumDeltaF += deltaF; // Se calcula deltaF promedio
c++;
avgDeltaF = sumDeltaF / c;
}
tres->MSE = pMSE;
tres->MSEHistory.push_back(tres->MSE);
tres->RMSE = getRMSE(inputs, targets);
tres->RMSEHistory.push_back(tres->RMSE);
tres->CE = getCE(inputs, targets);
tres->CEHistory.push_back(tres->CE);
// tr.layerWeightsHistory.push_back(layerWeights);
// tr.outputWeightsHistory.push_back(outputWeights);
// epc++;
if(sa){
if(fabs(avgDeltaF) < startCondition && c > 999){
// double avgDeltaF = sumDeltaF / c;
// T = avgDeltaF / log(initialAcceptance);
// T = 1 / log(initialAcceptance) * avgDeltaF;
// T = deltaF / log(Pa);
// T = -deltaF;
// T = To;
T = To;
double fNew;
NewState ns;
// int n = 0;
double fOld = tres->MSE;
double rnd = 0;
do{
for(unsigned int i = 0; i < nChanges; i++){
ns = addNoise(minNoise, maxNoise);
fNew = getNewMSE(ns.newWeights, ns.newOutputWeights, inputs, targets);
deltaF = fNew - fOld;
Pa = exp(-deltaF/T);
rnd = randomNumber(0,1);
if(deltaF < 0
|| rnd < Pa
){
layerWeights = ns.newWeights;
outputWeights = ns.newOutputWeights;
fOld = getMSE(inputs, targets);
}
}
// T = T / (1 + n);
T = tempDecFactor*T;
// n++;
}while(T > Tmin);
c = 0;
sumDeltaF = 0;
}
}
tres->epochs++;
}while(((tres->MSE >= MSEmin && sc == MSE) ||
(tres->RMSE >= RMSEmin && sc == RMSE) ||
(tres->CE >= CEmin && sc == CE)) &&
tres->epochs < epochs &&
training);
training = false;
tres->time = double(clock() - t_ini)/CLOCKS_PER_SEC;
}
void
opengv::generateMulti2D2DCorrespondences(
const translation_t & position1,
const rotation_t & rotation1,
const translation_t & position2,
const rotation_t & rotation2,
const translations_t & camOffsets,
const rotations_t & camRotations,
size_t pointsPerCam,
double noise,
double outlierFraction,
std::vector<boost::shared_ptr<bearingVectors_t> > & multiBearingVectors1,
std::vector<boost::shared_ptr<bearingVectors_t> > & multiBearingVectors2,
std::vector<boost::shared_ptr<Eigen::MatrixXd> > & gt )
{
//initialize point-cloud
double minDepth = 4;
double maxDepth = 8;
for( size_t cam = 0; cam < camOffsets.size(); cam++ )
{
boost::shared_ptr<Eigen::MatrixXd> gt_sub(new Eigen::MatrixXd(3,pointsPerCam));
for( size_t i = 0; i < pointsPerCam; i++ )
gt_sub->col(i) = generateRandomPoint( maxDepth, minDepth );
gt.push_back(gt_sub);
}
//iterate through the cameras (pairs)
for( size_t cam = 0; cam < camOffsets.size(); cam++ )
{
//create the bearing-vector arrays for this camera
boost::shared_ptr<bearingVectors_t> bearingVectors1(new bearingVectors_t());
boost::shared_ptr<bearingVectors_t> bearingVectors2(new bearingVectors_t());
//get the offset and rotation of this camera
translation_t camOffset = camOffsets[cam];
rotation_t camRotation = camRotations[cam];
//now iterate through the points of that camera
for( size_t i = 0; i < (size_t) pointsPerCam; i++ )
{
//project a point into both viewpoint frames
point_t bodyPoint1 = rotation1.transpose()*(gt[cam]->col(i) - position1);
point_t bodyPoint2 = rotation2.transpose()*(gt[cam]->col(i) - position2);
//project that point into the cameras
bearingVectors1->push_back( camRotation.transpose()*(bodyPoint1 - camOffset) );
bearingVectors2->push_back( camRotation.transpose()*(bodyPoint2 - camOffset) );
//normalize the vectors
(*bearingVectors1)[i] = (*bearingVectors1)[i] / (*bearingVectors1)[i].norm();
(*bearingVectors2)[i] = (*bearingVectors2)[i] / (*bearingVectors2)[i].norm();
//add noise
if( noise > 0.0 )
{
(*bearingVectors1)[i] = addNoise(noise,(*bearingVectors1)[i]);
(*bearingVectors2)[i] = addNoise(noise,(*bearingVectors2)[i]);
}
}
//push back the stuff for this camera
multiBearingVectors1.push_back(bearingVectors1);
multiBearingVectors2.push_back(bearingVectors2);
}
//add outliers
size_t outliersPerCam = (size_t) floor(outlierFraction*pointsPerCam);
//iterate through the cameras
for(size_t cam = 0; cam < camOffsets.size(); cam++)
{
//get the offset and rotation of this camera
translation_t camOffset = camOffsets[cam];
rotation_t camRotation = camRotations[cam];
//add outliers
for(size_t i = 0; i < outliersPerCam; i++)
{
//generate a random point
point_t p = generateRandomPoint(8,4);
//transform that point into viewpoint 2 only
point_t bodyPoint = rotation2.transpose()*(p - position2);
//use as measurement (outlier)
(*multiBearingVectors2[cam])[i] =
camRotation.transpose()*(bodyPoint - camOffset);
//normalize
(*multiBearingVectors2[cam])[i] =
(*multiBearingVectors2[cam])[i] / (*multiBearingVectors2[cam])[i].norm();
}
}
}
void
opengv::generateRandom2D2DCorrespondences(
const translation_t & position1,
const rotation_t & rotation1,
const translation_t & position2,
const rotation_t & rotation2,
const translations_t & camOffsets,
const rotations_t & camRotations,
size_t numberPoints,
double noise,
double outlierFraction,
bearingVectors_t & bearingVectors1,
bearingVectors_t & bearingVectors2,
std::vector<int> & camCorrespondences1,
std::vector<int> & camCorrespondences2,
Eigen::MatrixXd & gt )
{
//initialize point-cloud
double minDepth = 4;
double maxDepth = 8;
for( size_t i = 0; i < (size_t) gt.cols(); i++ )
gt.col(i) = generateRandomPoint( maxDepth, minDepth );
//create the 2D3D-correspondences by looping through the cameras
size_t numberCams = camOffsets.size();
size_t camCorrespondence = 0;
for( size_t i = 0; i < (size_t) gt.cols(); i++ )
{
//get the camera transformation
translation_t camOffset = camOffsets[camCorrespondence];
rotation_t camRotation = camRotations[camCorrespondence];
//get the point in viewpoint 1
point_t bodyPoint1 = rotation1.transpose()*(gt.col(i) - position1);
//get the point in viewpoint 2
point_t bodyPoint2 = rotation2.transpose()*(gt.col(i) - position2);
//get the point in the camera in viewpoint 1
bearingVectors1.push_back(camRotation.transpose()*(bodyPoint1 - camOffset));
//get the point in the camera in viewpoint 2
bearingVectors2.push_back(camRotation.transpose()*(bodyPoint2 - camOffset));
//normalize the bearing-vectors
bearingVectors1[i] = bearingVectors1[i] / bearingVectors1[i].norm();
bearingVectors2[i] = bearingVectors2[i] / bearingVectors2[i].norm();
//add noise
if( noise > 0.0 )
{
bearingVectors1[i] = addNoise(noise,bearingVectors1[i]);
bearingVectors2[i] = addNoise(noise,bearingVectors2[i]);
}
//push back the camera correspondences
camCorrespondences1.push_back(camCorrespondence);
camCorrespondences2.push_back(camCorrespondence++);
if(camCorrespondence > (numberCams - 1) )
camCorrespondence = 0;
}
//add outliers
size_t numberOutliers = (size_t) floor(outlierFraction*numberPoints);
for(size_t i = 0; i < numberOutliers; i++)
{
//get the corresponding camera transformation
translation_t camOffset = camOffsets[camCorrespondence];
rotation_t camRotation = camRotations[camCorrespondence];
//create random point
point_t p = generateRandomPoint(8,4);
//project this point into viewpoint 2
point_t bodyPoint = rotation2.transpose()*(p - position2);
//project this point into the corresponding camera in viewpoint 2
//and use as outlier
bearingVectors2[i] = camRotation.transpose()*(bodyPoint - camOffset);
//normalize the bearing vector
bearingVectors2[i] = bearingVectors2[i] / bearingVectors2[i].norm();
}
}
void
opengv::generateRandom2D3DCorrespondences(
const translation_t & position,
const rotation_t & rotation,
const translations_t & camOffsets,
const rotations_t & camRotations,
size_t numberPoints,
double noise,
double outlierFraction,
bearingVectors_t & bearingVectors,
points_t & points,
std::vector<int> & camCorrespondences,
Eigen::MatrixXd & gt )
{
//initialize point-cloud
double minDepth = 4;
double maxDepth = 8;
for( size_t i = 0; i < (size_t) gt.cols(); i++ )
gt.col(i) = generateRandomPoint( maxDepth, minDepth );
//create the 2D3D-correspondences by looping through the cameras
size_t numberCams = camOffsets.size();
size_t camCorrespondence = 0;
for( size_t i = 0; i < (size_t) gt.cols(); i++ )
{
//get the camera transformation
translation_t camOffset = camOffsets[camCorrespondence];
rotation_t camRotation = camRotations[camCorrespondence];
//store the point
points.push_back(gt.col(i));
//project the point into the viewpoint frame
point_t bodyPoint = rotation.transpose()*(gt.col(i) - position);
//project the point into the camera frame
bearingVectors.push_back(camRotation.transpose()*(bodyPoint - camOffset));
//normalize the bearing-vector to 1
bearingVectors[i] = bearingVectors[i] / bearingVectors[i].norm();
//add noise
if( noise > 0.0 )
bearingVectors[i] = addNoise(noise,bearingVectors[i]);
//push back the camera correspondence
camCorrespondences.push_back(camCorrespondence++);
if(camCorrespondence > (numberCams-1) )
camCorrespondence = 0;
}
//add outliers
//compute the number of outliers based on fraction
size_t numberOutliers = (size_t) floor(outlierFraction*numberPoints);
//make the first numberOutliers points be outliers
for(size_t i = 0; i < numberOutliers; i++)
{
//extract the camera transformation
translation_t camOffset = camOffsets[camCorrespondences[i]];
rotation_t camRotation = camRotations[camCorrespondences[i]];
//create random point
point_t p = generateRandomPoint(8,4);
//project into viewpoint frame
point_t bodyPoint = rotation.transpose()*(p - position);
//project into camera-frame and use as outlier measurement
bearingVectors[i] = camRotation.transpose()*(bodyPoint - camOffset);
//normalize the bearing vector
bearingVectors[i] = bearingVectors[i] / bearingVectors[i].norm();
}
}
ReturnFlag CompositionDBG::evaluate_(VirtualEncoding &ss, bool rFlag, ProgramMode mode, bool flag_){
CodeVReal &s=dynamic_cast<CodeVReal &>(ss);
double *x=new double[m_numDim];
copy(s.m_x.begin(),s.m_x.end(),x);
if(this->m_noiseFlag) addNoise(x);
vector<double> width(m_numPeaks,0),fit(m_numPeaks);
for(int i=0;i<m_numPeaks;i++){ // calculate weight for each function
for(int j=0;j<m_numDim;j++)
width[i]+=(x[j]-mpp_peak[i][j])*(x[j]-mpp_peak[i][j]);
if(width[i]!=0) width[i]=exp(-sqrt(width[i]/(2*m_numDim*mp_convergeSeverity[i]*mp_convergeSeverity[i])));
}
for(int i=0;i<m_numPeaks;i++){ // calculate objective value for each function
for(int j=0;j<m_numDim;j++) // calculate the objective value of tranformation function i
x[j]=(x[j]-mpp_peak[i][j])/mp_stretchSeverity[i];//((1+fabs(mpp_peak[i][j]/mp_searchRange[j].m_upper))*
Matrix m(m_numDim,1);
m.setDataRow(x,m_numDim);
m*=mp_rotationMatrix[i];
copy(m[0].begin(),m[0].end(),x);
correctSolution(mp_comFunction[i],x);
fit[i]=selectFun(mp_comFunction[i],x);
fit[i]=m_heightNormalizeSeverity*fit[i]/fabs(mp_fmax[i]);
copy(s.m_x.begin(),s.m_x.end(),x);
}
double sumw=0,wmax;
wmax=*max_element(width.begin(),width.end());
for(int i=0;i<m_numPeaks;i++)
if(width[i]!=wmax)
width[i]=width[i]*(1-pow(wmax,10));
for(int i=0;i<m_numPeaks;i++)
sumw+=width[i];
for(int i=0;i<m_numPeaks;i++)
width[i]/=sumw;
double obj=0;
for(int i=0;i<m_numPeaks;i++)
obj+=width[i]*(fit[i]+mp_height[i]);
s.m_obj[0]=obj;
if(rFlag&&m_evals%m_changeFre==0) Solution<CodeVReal>::initilizeWB(s);
if(rFlag){
isTracked(x,s.m_obj);
m_evals++;
}
bool flag;
#ifdef OFEC_CONSOLE
if(Global::msp_global->mp_algorithm!=nullptr) flag=!Global::msp_global->mp_algorithm->ifTerminating();
else flag=true;
#endif
#ifdef OFEC_DEMON
flag=true;
#endif
if(rFlag&&m_evals%m_changeFre==0&&flag) {
DynamicProblem::change();
if(m_timeLinkageFlag) updateTimeLinkage();
}
delete []x;
x=0;
ReturnFlag rf=Return_Normal;
if(rFlag){
if(Global::msp_global->mp_algorithm!=nullptr){
if(Global::msp_global->mp_algorithm->ifTerminating()){ rf=Return_Terminate; }
else if(Global::msp_global->mp_problem->isProTag(DOP)){
if(CAST_PROBLEM_DYN->getFlagTimeLinkage()&&CAST_PROBLEM_DYN->getTriggerTimelinkage()){
rf=Return_Change_Timelinkage;
}
if((Global::msp_global->mp_problem->getEvaluations()+1)%(CAST_PROBLEM_DYN->getChangeFre())==0){
rf=Return_ChangeNextEval;
}
if(Global::msp_global->mp_problem->getEvaluations()%(CAST_PROBLEM_DYN->getChangeFre())==0){
if(CAST_PROBLEM_DYN->getFlagDimensionChange()){
rf=Return_Change_Dim;
}
rf=Return_Change;
}
}
}
}
return rf;
}
ReturnFlag MovingPeak::evaluate_(VirtualEncoding &ss, bool rFlag, ProgramMode mode, bool flag2){
CodeVReal &s=dynamic_cast<CodeVReal &>(ss);
double *x=new double[m_numDim];
copy(s.m_x.begin(),s.m_x.end(),x);
if(this->m_noiseFlag) addNoise(x);
double maximum = LONG_MIN, dummy;
for(int i=0; i<m_numPeaks; i++){
//if(maximum>mp_height[i]) continue; //optimization on the obj evaluation
dummy = functionSelection(x, i);
if (dummy > maximum) maximum = dummy;
}
if (m_useBasisFunction){
dummy = functionSelection(x,-1);
/* If value of basis function is higher return it */
if (maximum < dummy) maximum = dummy;
}
s.m_obj[0]=maximum;
if(rFlag&&m_evals%m_changeFre==0){
Solution<CodeVReal>::initilizeWB(s);
}
if(rFlag&&isTracked(x,s.m_obj)) updatePeakQaulity();
if(rFlag) m_evals++;
bool flag;
#ifdef OFEC_CONSOLE
if(Global::msp_global->mp_algorithm!=nullptr) flag=!Global::msp_global->mp_algorithm->ifTerminating();
else flag=true;
#endif
#ifdef OFEC_DEMON
flag=true;
#endif
if(rFlag&&m_evals%m_changeFre==0&&flag){
//g_mutexStream.lock();
//cout<<"The number of changes: "<<m_changeCounter<<endl;
//g_mutexStream.unlock();
//for(int i=0;i<m_numPeaks;i++) printPeak(i);
DynamicProblem::change();
//for(int i=0;i<m_numPeaks;i++) printPeak(i);
//getchar();
}
delete [] x;
x=0;
ReturnFlag rf=Return_Normal;
if(rFlag){
if(Global::msp_global->mp_algorithm!=nullptr){
if(Global::msp_global->mp_algorithm->ifTerminating()){ rf=Return_Terminate; }
else if(Global::msp_global->mp_problem->isProTag(DOP)){
if(CAST_PROBLEM_DYN->getFlagTimeLinkage()&&CAST_PROBLEM_DYN->getTriggerTimelinkage()){
rf=Return_Change_Timelinkage;
}
if((Global::msp_global->mp_problem->getEvaluations()+1)%(CAST_PROBLEM_DYN->getChangeFre())==0){
rf=Return_ChangeNextEval;
}
if(Global::msp_global->mp_problem->getEvaluations()%(CAST_PROBLEM_DYN->getChangeFre())==0){
if(CAST_PROBLEM_DYN->getFlagDimensionChange()){
rf=Return_Change_Dim;
}
rf=Return_Change;
}
}
}
}
return rf;
}
// This gets called by the world update start event.
void GazeboImuPlugin::OnUpdate(const common::UpdateInfo& _info) {
common::Time current_time = world_->GetSimTime();
double dt = (current_time - last_time_).Double();
last_time_ = current_time;
double t = current_time.Double();
math::Pose T_W_I = link_->GetWorldPose(); //TODO(burrimi): Check tf.
math::Quaternion C_W_I = T_W_I.rot;
// Copy math::Quaternion to gazebo::msgs::Quaternion
gazebo::msgs::Quaternion* orientation = new gazebo::msgs::Quaternion();
orientation->set_x(C_W_I.x);
orientation->set_y(C_W_I.y);
orientation->set_z(C_W_I.z);
orientation->set_w(C_W_I.w);
#if GAZEBO_MAJOR_VERSION < 5
math::Vector3 velocity_current_W = link_->GetWorldLinearVel();
// link_->GetRelativeLinearAccel() does not work sometimes with old gazebo versions.
// TODO For an accurate simulation, this might have to be fixed. Consider the
// This issue is solved in gazebo 5.
math::Vector3 acceleration = (velocity_current_W - velocity_prev_W_) / dt;
math::Vector3 acceleration_I =
C_W_I.RotateVectorReverse(acceleration - gravity_W_);
velocity_prev_W_ = velocity_current_W;
#else
math::Vector3 acceleration_I = link_->GetRelativeLinearAccel() - C_W_I.RotateVectorReverse(gravity_W_);
#endif
math::Vector3 angular_vel_I = link_->GetRelativeAngularVel();
Eigen::Vector3d linear_acceleration_I(acceleration_I.x,
acceleration_I.y,
acceleration_I.z);
Eigen::Vector3d angular_velocity_I(angular_vel_I.x,
angular_vel_I.y,
angular_vel_I.z);
addNoise(&linear_acceleration_I, &angular_velocity_I, dt);
// Copy Eigen::Vector3d to gazebo::msgs::Vector3d
gazebo::msgs::Vector3d* linear_acceleration = new gazebo::msgs::Vector3d();
linear_acceleration->set_x(linear_acceleration_I[0]);
linear_acceleration->set_y(linear_acceleration_I[1]);
linear_acceleration->set_z(linear_acceleration_I[2]);
// Copy Eigen::Vector3d to gazebo::msgs::Vector3d
gazebo::msgs::Vector3d* angular_velocity = new gazebo::msgs::Vector3d();
angular_velocity->set_x(angular_velocity_I[0]);
angular_velocity->set_y(angular_velocity_I[1]);
angular_velocity->set_z(angular_velocity_I[2]);
// Fill IMU message.
// ADD HEaders
// imu_message_.header.stamp.sec = current_time.sec;
// imu_message_.header.stamp.nsec = current_time.nsec;
// TODO(burrimi): Add orientation estimator.
// imu_message_.orientation.w = 1;
// imu_message_.orientation.x = 0;
// imu_message_.orientation.y = 0;
// imu_message_.orientation.z = 0;
imu_message_.set_allocated_orientation(orientation);
imu_message_.set_allocated_linear_acceleration(linear_acceleration);
imu_message_.set_allocated_angular_velocity(angular_velocity);
imu_pub_->Publish(imu_message_);
}
/**
* argv[1] = path to training ply file
* argv[2] = hypothesis id
* argv[3] = path to omap save directory
* argv[4] = vpx
* argv[5] = vpy
* argv[6] = vpz
* argv[7] = vp_id
*/
int build_omap_main(int argc, char **argv)
{
std::cout << "Starting oMap build process..." << std::endl;
if(argc < 4)
throw std::runtime_error("A directory to save the confMat.txt is required...\n");
if(argc < 3)
throw std::runtime_error("Hypothesis id is required as second argument...\n");
if(argc < 2)
throw std::runtime_error("A ply file is required as a first argument...\n");
// Get directory paths and hypothesis id
std::string ply_file_path( argv[1] );
std::string hyp_id( argv[2] );
std::string save_dir_path( argv[3] );
std::string vision_module_path("/home/bharath/GRASP_Code/ns_shared/penn_nbv_slam/vision_module");
// Form the output filename
std::stringstream omap_file_path;
if(argc > 4){
omap_file_path << save_dir_path << "/confMatorg_" << hyp_id << "_" << argv[7] << ".txt";
}
else{
omap_file_path << save_dir_path << "/confMatorg_" << hyp_id << ".txt";
}
std::ofstream outfile ( omap_file_path.str().c_str(), ofstream::app );
if(!outfile.is_open())
throw std::runtime_error("Unable to open conf mat save file...\n");
// Import the sensor locations
int num_vp;
int dim_vp = 3;
Eigen::MatrixXd vp_positions;
num_vp = 1;
vp_positions.resize( num_vp, dim_vp );
double vpx = std::atof(argv[4]);
double vpy = std::atof(argv[5]);
double vpz = std::atof(argv[6]);
vp_positions << vpx, vpy, vpz;
// Construct and initialize the virtual kinect
int coord = 1; // 0 = obj coord, 1 = optical sensor coord, 2 = standard sensor coord
bool add_noise = false;
vkin_offline vko( Eigen::Vector3d(0,0,0), Eigen::Vector4d(0,0,0,1),
coord, false, add_noise );
vko.init_vkin( ply_file_path );
Eigen::Vector3d target(0,0,0);
// Construct and start a vocabulary tree
vtree_user vt("/load", vision_module_path + "/data", vision_module_path + "/../data/cloud_data/");
vt.start();
// noise
boost::mt19937 rng(time(0));
boost::normal_distribution<double> normal_distrib(0.0, 1.0);
boost::variate_generator<boost::mt19937&, boost::normal_distribution<double> > gaussian_rng( rng, normal_distrib );
double vp_noise_param = 0.04; // standard deviation in position
double tp_noise_param = 0.015; // standard deviation in target
int num_rep = 20;
for(int vp = 0; vp < num_vp; ++vp)
{
for(int r = 0; r < num_rep; ++r)
{
// get cloud copy
Eigen::Vector3d noise_vp;
noise_vp.x() = vp_positions(vp,0) + vp_noise_param*gaussian_rng();
noise_vp.y() = vp_positions(vp,1) + vp_noise_param*gaussian_rng();
noise_vp.z() = vp_positions(vp,2) + vp_noise_param*gaussian_rng();
Eigen::Vector3d noise_tp;
noise_tp.x() = target.x() + tp_noise_param*gaussian_rng();
noise_tp.y() = target.y() + tp_noise_param*gaussian_rng();
noise_tp.z() = target.z() + tp_noise_param*gaussian_rng();
pcl::PointCloud<pcl::PointXYZ>::Ptr noise_cloud_ptr = vko.sense( noise_vp, noise_tp );
//add noise
addNoise( Eigen::Vector3d(0,0,0), noise_cloud_ptr, gaussian_rng );
std::pair<float,std::string> vp_score = vt.top_match( noise_cloud_ptr->getMatrixXfMap() );
// append vp and vp_score to the file
outfile << argv[1]<<" "<<argv[7] << " " << vp_score.second << " " << vp_score.first << std::endl;
}
}
outfile.close();
return 0;
}
/**
* argv[1] = path to training ply file
* argv[2] = hypothesis id
* argv[3] = path to omap save directory
* argv[4] = vpx
* argv[5] = vpy
* argv[6] = vpz
* argv[7] = vp_id
*/
int build_omap_main(int argc, char **argv)
{
std::cout << "Starting oMap build process..." << std::endl;
if(argc < 4)
throw std::runtime_error("A directory to save the oMap_hyp.txt is required...\n");
if(argc < 3)
throw std::runtime_error("Hypothesis id is required as second argument...\n");
if(argc < 2)
throw std::runtime_error("A ply file is required as a first argument...\n");
// Get directory paths and hypothesis id
std::string ply_file_path( argv[1] );
std::string hyp_id( argv[2] );
std::string save_dir_path( argv[3] );
std::string node_id("build_omap");
std::string vision_module_path(ros::package::getPath("vision_module"));
// Form the output filename
std::stringstream omap_file_path;
if(argc > 4) {
omap_file_path << save_dir_path << "/oMap_" << hyp_id << "_" << argv[7] << ".txt";
node_id.append( hyp_id + argv[7] );
}
else {
omap_file_path << save_dir_path << "/oMap_" << hyp_id << ".txt";
node_id.append( hyp_id );
}
std::ofstream outfile ( omap_file_path.str().c_str(), ofstream::app );
if(!outfile.is_open())
throw std::runtime_error("Unable to open omap save file...\n");
// Import the sensor locations
int num_vp;
int dim_vp = 3;
Eigen::MatrixXd vp_positions;
if(argc > 4) {
num_vp = 1;
vp_positions.resize( num_vp, dim_vp );
double vpx = std::atof(argv[4]);
double vpy = std::atof(argv[5]);
double vpz = std::atof(argv[6]);
vp_positions << vpx, vpy, vpz;
} else {
std::string vp_file_path(vision_module_path + "/data/omap_vps.txt");
io_utils::file2matrix( vp_file_path, vp_positions, dim_vp );
num_vp = vp_positions.rows();
}
// Construct and initialize the virtual kinect
int coord = 1; // 0 = obj coord, 1 = optical sensor coord, 2 = standard sensor coord
bool add_noise = false;
vkin_offline vko( Eigen::Vector3d(0,0,0), Eigen::Vector4d(0,0,0,1),
coord, false, add_noise );
vko.init_vkin( ply_file_path );
Eigen::Vector3d target(0,0,0);
// Construct and start a vocabulary tree
vtree_user vt("/load", vision_module_path + "/data", vision_module_path + "/../data/cloud_data");
vt.start();
// noise
boost::mt19937 rng(time(0));
boost::normal_distribution<double> normal_distrib(0.0, 1.0);
boost::variate_generator<boost::mt19937&, boost::normal_distribution<double> > gaussian_rng( rng, normal_distrib );
double vp_noise_param = 0.01; // standard deviation in position
double tp_noise_param = 0.005; // standard deviation in target
double occ_dev = 0.005; // occlussion cutoff threshold in the sensor frame
int num_rep = 2;
for(int vp = 0; vp < num_vp; ++vp)
{
// get a noiseless scan in the sensor frame
//pcl::PointCloud<pcl::PointXYZ>::Ptr cld_ptr = vko.sense( vp_positions.row(vp).transpose(), target );
for(int r = 0; r < num_rep; ++r)
{
// get cloud copy
//pcl::PointCloud<pcl::PointXYZ>::Ptr noise_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
//copyPointCloud( *cld_ptr, *noise_cloud_ptr );
Eigen::Vector3d noise_vp;
noise_vp.x() = vp_positions(vp,0) + vp_noise_param*gaussian_rng();
noise_vp.y() = vp_positions(vp,1) + vp_noise_param*gaussian_rng();
noise_vp.z() = vp_positions(vp,2) + vp_noise_param*gaussian_rng();
Eigen::Vector3d noise_tp;
noise_tp.x() = target.x() + tp_noise_param*gaussian_rng();
noise_tp.y() = target.y() + tp_noise_param*gaussian_rng();
noise_tp.z() = target.z() + tp_noise_param*gaussian_rng();
pcl::PointCloud<pcl::PointXYZ>::Ptr cld_ptr = vko.sense( noise_vp, noise_tp );
// get occluded pcds
pcl::PointCloud<pcl::PointXYZ>::Ptr cld_ptr_1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr l_cld_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr r_cld_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr t_cld_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr b_cld_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr lr_cld_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr tb_cld_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cld_ptr, *cld_ptr_1);
pcl::copyPointCloud(*cld_ptr, *l_cld_ptr);
pcl::copyPointCloud(*cld_ptr, *r_cld_ptr);
pcl::copyPointCloud(*cld_ptr, *t_cld_ptr);
pcl::copyPointCloud(*cld_ptr, *b_cld_ptr);
pcl::copyPointCloud(*cld_ptr, *lr_cld_ptr);
pcl::copyPointCloud(*cld_ptr, *tb_cld_ptr);
// occlude the other two clouds too
occlude_pcd(cld_ptr, 0, -3*occ_dev, 3*occ_dev); // left-right
occlude_pcd(cld_ptr_1, 1, -3*occ_dev, 3*occ_dev); // top-bottom
occlude_pcd(l_cld_ptr, 0, -occ_dev, std::numeric_limits<double>::infinity());
occlude_pcd(r_cld_ptr, 0, -std::numeric_limits<double>::infinity(), occ_dev);
occlude_pcd(t_cld_ptr, 1, -occ_dev, std::numeric_limits<double>::infinity());
occlude_pcd(b_cld_ptr, 1, -std::numeric_limits<double>::infinity(), occ_dev);
occlude_pcd(lr_cld_ptr, 0, -2.5*occ_dev, 2.5*occ_dev);
occlude_pcd(tb_cld_ptr, 1, -2.5*occ_dev, 2.5*occ_dev);
//add noise
addNoise( Eigen::Vector3d(0,0,0), cld_ptr, gaussian_rng );
addNoise( Eigen::Vector3d(0,0,0), cld_ptr_1, gaussian_rng );
addNoise( Eigen::Vector3d(0,0,0), l_cld_ptr, gaussian_rng );
addNoise( Eigen::Vector3d(0,0,0), r_cld_ptr, gaussian_rng );
addNoise( Eigen::Vector3d(0,0,0), t_cld_ptr, gaussian_rng );
addNoise( Eigen::Vector3d(0,0,0), b_cld_ptr, gaussian_rng );
addNoise( Eigen::Vector3d(0,0,0), lr_cld_ptr, gaussian_rng );
addNoise( Eigen::Vector3d(0,0,0), tb_cld_ptr, gaussian_rng );
/*
//filter
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> stat;
stat.setInputCloud( noise_cloud_ptr );
stat.setMeanK(100);
stat.setStddevMulThresh(1.0);
stat.filter( *noise_cloud_ptr );
*/
// Record scores
std::pair<float,std::string> vp_score = vt.top_match( cld_ptr->getMatrixXfMap() );
std::pair<float,std::string> vp_score_1 = vt.top_match( cld_ptr_1->getMatrixXfMap() );
std::pair<float,std::string> vp_score_l = vt.top_match( l_cld_ptr->getMatrixXfMap() );
std::pair<float,std::string> vp_score_r = vt.top_match( r_cld_ptr->getMatrixXfMap() );
std::pair<float,std::string> vp_score_t = vt.top_match( t_cld_ptr->getMatrixXfMap() );
std::pair<float,std::string> vp_score_b = vt.top_match( b_cld_ptr->getMatrixXfMap() );
std::pair<float,std::string> vp_score_lr = vt.top_match( lr_cld_ptr->getMatrixXfMap() );
std::pair<float,std::string> vp_score_tb = vt.top_match( tb_cld_ptr->getMatrixXfMap() );
// append vp and vp_score to the file
if(argc > 4)
{
outfile << argv[7] << " " << vp_score.second << " " << vp_score.first << std::endl;
outfile << argv[7] << " " << vp_score_1.second << " " << vp_score_1.first << std::endl;
outfile << argv[7] << " " << vp_score_l.second << " " << vp_score_l.first << std::endl;
outfile << argv[7] << " " << vp_score_r.second << " " << vp_score_r.first << std::endl;
outfile << argv[7] << " " << vp_score_t.second << " " << vp_score_t.first << std::endl;
outfile << argv[7] << " " << vp_score_b.second << " " << vp_score_b.first << std::endl;
outfile << argv[7] << " " << vp_score_lr.second << " " << vp_score_lr.first << std::endl;
outfile << argv[7] << " " << vp_score_tb.second << " " << vp_score_tb.first << std::endl;
}
else
{
outfile << vp << " " << vp_score.second << " " << vp_score.first << std::endl;
outfile << vp << " " << vp_score_1.second << " " << vp_score_1.first << std::endl;
outfile << vp << " " << vp_score_l.second << " " << vp_score_l.first << std::endl;
outfile << vp << " " << vp_score_r.second << " " << vp_score_r.first << std::endl;
outfile << vp << " " << vp_score_t.second << " " << vp_score_t.first << std::endl;
outfile << vp << " " << vp_score_b.second << " " << vp_score_b.first << std::endl;
outfile << vp << " " << vp_score_lr.second << " " << vp_score_lr.first << std::endl;
outfile << vp << " " << vp_score_tb.second << " " << vp_score_tb.first << std::endl;
}
/*
// save clouds for inspection
if((vp == 0) && (r == 0))
{
pcl::PCDWriter writer;
std::string pcd_file_path ("/media/natanaso/Data/Stuff/Research/code_develop_area/cpp_test_pkg/data/l_occ1.pcd");
writer.writeASCII( pcd_file_path.c_str(), *l_cld_ptr );
pcd_file_path = "/media/natanaso/Data/Stuff/Research/code_develop_area/cpp_test_pkg/data/r_occ1.pcd";
writer.writeASCII( pcd_file_path.c_str(), *r_cld_ptr );
pcd_file_path = "/media/natanaso/Data/Stuff/Research/code_develop_area/cpp_test_pkg/data/t_occ1.pcd";
writer.writeASCII( pcd_file_path.c_str(), *t_cld_ptr );
pcd_file_path = "/media/natanaso/Data/Stuff/Research/code_develop_area/cpp_test_pkg/data/b_occ1.pcd";
writer.writeASCII( pcd_file_path.c_str(), *b_cld_ptr );
pcd_file_path = "/media/natanaso/Data/Stuff/Research/code_develop_area/cpp_test_pkg/data/lr_occ1.pcd";
writer.writeASCII( pcd_file_path.c_str(), *lr_cld_ptr );
pcd_file_path = "/media/natanaso/Data/Stuff/Research/code_develop_area/cpp_test_pkg/data/tb_occ1.pcd";
writer.writeASCII( pcd_file_path.c_str(), *tb_cld_ptr );
pcd_file_path = "/media/natanaso/Data/Stuff/Research/code_develop_area/cpp_test_pkg/data/orig1.pcd";
writer.writeASCII( pcd_file_path.c_str(), *cld_ptr );
}
*/
}
}
outfile.close();
return 0;
}
