void pushSelfAndAdjacentCells(int x, int y){
for(int i = y + 1; i >= y - 1; --i){
for(int j = x + 1; j >= x - 1; --j){
if(!outOfBounds(j) && !outOfBounds(i))
floodStack.push(liveMaze[j][i]);
}
}
}
void Snake::update(sf::Time delta)
{
if (life_ <= 0)
return;
for (auto it = bullets_.begin(); it != bullets_.end(); ++it)
{
if (it->outOfBounds())
bulletsToErase_.push_back(std::distance(bullets_.begin(), it));
checkBulletCollisions(std::distance(bullets_.begin(), it));
it->update(delta);
}
for (auto& it: bulletsToErase_)
{
if (it != bullets_.size())
bullets_.erase(bullets_.begin() + it);
}
bulletsToErase_.clear();
checkEdgeCollisions();
checkEnemyBulletCollisions();
move(delta);
canShoot_ = fireClock_.getElapsedTime() - lastFireTime_ >= FireRate;
particleSystem_.update(delta);
}
void ImageProcessing::pointReduction(Image *non_hole_filled_map, Image *hole_filled_map, int radius) {
Image *markup_image = new Image(non_hole_filled_map->getWidth(), non_hole_filled_map->getHeight(), 0.0f, 0.0f, 0.0f, 1.0f);
///Go through all the valid points in the non-hole-filled image and mark a neighbourhood of radius around them.
for (int y=0;y<non_hole_filled_map->getHeight();y++) {
for (int x=0;x<non_hole_filled_map->getWidth();x++) {
///If there is no point then ignore
if (non_hole_filled_map->getPixel(x,y) == Color(0.0f,0.0f,0.0f)) continue;
///Otherwise mark a neighbourhood of radius around it
for (int i=-radius/2;i<radius/2;i++) {
for (int j=-radius/2;j<radius/2;j++) {
if (outOfBounds(markup_image, x+j, y+i)) continue;
markup_image->setPixel(x+j, y+i, Color(1.0f,1.0f,1.0f));
}
}
}
}
///Go through the hole-filled image and keep only the points in the markup image
for (int y=0;y<hole_filled_map->getHeight();y++) {
for (int x=0;x<hole_filled_map->getWidth();x++) {
if (markup_image->getPixel(x,y) == Color(0.0f,0.0f,0.0f)) {
hole_filled_map->setPixel(x,y,Color(0.0f,0.0f,0.0f));
}
}
}
///Clean up
delete markup_image;
return;
}
JNIEXPORT jdouble JNICALL Java_neuron_Neuron_vectorGet
(JNIEnv *env, jclass, jlong jc, jint i){
Object* ho = (Object*)jc;
Vect* vec = (Vect*)ho->u.this_pointer;
if (i < 0 || i >= vec->capacity()) {
printf("Neuron.vectorGet i=%d size=%d\n", i, vec->capacity());
outOfBounds(env);
}
return vec->elem(i);
}
void Experiment::timer_cb(const ros::TimerEvent& event)
{
switch(mode_)
{
case MODE_REP_START:
actions_->Start();
last_time_ = ros::Time::now().toSec();
state_ = states_->GetState();
action_ = qobj_->GetAction(state_);
actions_->Move(action_);
ROS_INFO("Starting rep: %d", cnt_rep_);
mode_ = MODE_REP;
cnt_timesteps_++;
break;
case MODE_REP:
state_p_ = (int)states_->GetState();
if (learn_)
{
reward_ = states_->GetReward();
qobj_->Update(reward_, state_, state_p_, action_);
ROS_INFO("Action: %d, produced state: %d with reward: %f", action_, state_p_, reward_);
ROS_INFO("Table: \n%s", qobj_->PrintTable().c_str());
}
state_ = state_p_;
action_ = qobj_->GetAction(state_);
actions_->Move(action_);
cnt_timesteps_++;
if (getDistance() < goal_radius_ || outOfBounds())
{
stopAndMoveToStart();
}
break;
case MODE_RETURN:
if (move_stopped_ == true)
{
move_stopped_ = false;
mode_ = MODE_REP_START;
qobj_->DecreaseTemp();
cnt_rep_++;
}
if (cnt_rep_ > num_reps_)
mode_ = MODE_DONE;
break;
case MODE_DONE:
//lc_->ShowImage();
exit(1);
break;
}
}
// "full2compact()": compacts a 3D image tract into vector form
void fileManager::full2compact(const std::vector< std::vector< std::vector <float> > >& fullTract, std::vector<float>* compactPointer ) const
{
if (m_maskMatrix.empty())
{
throw std::runtime_error("ERROR @ fileManager::full2compact(): Mask hast not been loaded, tract has not been processed");
}
const size_t dimx = fullTract.size();
const size_t dimy = fullTract[0].size();
const size_t dimz = fullTract[0][0].size();
if ( dimx != m_maskMatrix.size() || dimy != m_maskMatrix[0].size() || dimz!= m_maskMatrix[0][0].size() )
{
throw std::runtime_error("ERROR @ fileManager::full2compact(): mask and full tract dimensions do not match");
}
std::vector<float>& compact( *compactPointer );
compact.clear();
compact.reserve( getTractSize() );
size_t inBounds( 0 ), outOfBounds( 0 );
for( int i=0 ; i<dimz ; ++i )
{
for( int j=0 ; j<dimy ; ++j )
{
for( int k=0 ; k<dimx ; ++k )
{
float datapoint( fullTract[k][j][i] );
if( m_maskMatrix[k][j][i] )
{
compact.push_back( datapoint );
if( datapoint )
{
++inBounds;
}
}
else
{
if( datapoint )
{
++outOfBounds;
}
}
}
}
}
if( outOfBounds > 0 )
{
std::cerr << "WARNING @ fileManager::full2compact(): full tract had " << outOfBounds << " non-zero voxels outside of mask (and " << inBounds << " non-zero voxels within the mask)" << std::endl;
}
if( getTractSize() != compact.size() )
{
std::cerr << "Tract lenght: " << compact.size() << ". Mask length: " << getTractSize() << std::endl;
throw std::runtime_error( "fileManager::full2compact(): Mask and tractogram sizes do not match" );
}
return;
}// end fileManager::full2compact() -----------------------------------------------------------------
/**
* Main program.
*/
int main
(int argc,
char **argv)
{
int i, r, c;
long long int t1, t2;
// Initialize MPI middleware.
MPI_Init (&argc, &argv);
world = MPI_COMM_WORLD;
MPI_Comm_size (world, &size);
MPI_Comm_rank (world, &rank);
// Parse command line arguments.
if (argc != 2) usage();
sscanf (argv[1], "%d", &n);
// Allocate distance matrix.
allocateDistanceMatrix();
// Run Floyd's Algorithm.
// for i = 0 to N-1
// for r = 0 to N-1
// for c = 0 to N-1
// D[r,c] = min (D[r,c], D[r,i] + D[i,c])
t1 = currentTimeMillis();
for (i = 0; i < n; ++ i)
{
double *d_i;
if (i < 0 || i >= n) outOfBounds();
d_i = d[i];
for (r = 0; r < n; ++ r)
{
double *d_r;
if (r < 0 || r >= n) outOfBounds();
d_r = d[r];
for (c = 0; c < n; ++ c)
{
double d_r_c, d_r_i, d_i_c;
if (c < 0 || c >= n) outOfBounds();
d_r_c = d_r[c];
if (i < 0 || i >= n) outOfBounds();
d_r_i = d_r[i];
if (c < 0 || c >= n) outOfBounds();
d_i_c = d_i[c];
if (c < 0 || c >= n) outOfBounds();
d_r[c] = min (d_r_c, d_r_i + d_i_c);
}
}
}
t2 = currentTimeMillis();
// Print running time.
printf ("%lld msec\n", t2-t1);
// Finalize MPI middleware.
MPI_Finalize();
}
void Map::place(Voxel& block)
{
table[index3D(block.getMapX(), block.getMapY(), block.getMapZ())] = block;
Voxel* b = &table[index3D(block.getMapX(), block.getMapY(), block.getMapZ())];
int x = b->getMapX();
int y = b->getMapY();
int z = b->getMapZ();
if(&table[index3D(x, y, z)] != nullptr)
{
voxelBatch->removeVoxel(&table[index3D(x, y, z)]);
}
if (!b->isAir())
{
voxelBatch->addVoxel(b);
if (!outOfBounds(x + 1, y, z) && !table[index3D(x + 1, y, z)].isAir())
{
voxelBatch->removeVoxelFace(b, VOXEL_FACE_RIGHT);
voxelBatch->removeVoxelFace(&table[index3D(x + 1, y, z)], VOXEL_FACE_LEFT);
}
if (!outOfBounds(x - 1, y, z) && !table[index3D(x - 1, y, z)].isAir())
{
voxelBatch->removeVoxelFace(b, VOXEL_FACE_LEFT);
voxelBatch->removeVoxelFace(&table[index3D(x - 1, y, z)], VOXEL_FACE_RIGHT);
}
if (!outOfBounds(x, y + 1, z) && !table[index3D(x, y + 1, z)].isAir())
{
voxelBatch->removeVoxelFace(b, VOXEL_FACE_TOP);
voxelBatch->removeVoxelFace(&table[index3D(x, y + 1, z)], VOXEL_FACE_BOTTOM);
}
if (!outOfBounds(x, y - 1, z) && !table[index3D(x, y - 1, z)].isAir())
{
voxelBatch->removeVoxelFace(b, VOXEL_FACE_BOTTOM);
voxelBatch->removeVoxelFace(&table[index3D(x, y - 1, z)], VOXEL_FACE_TOP);
}
if (!outOfBounds(x, y, z + 1) && !table[index3D(x, y, z + 1)].isAir())
{
voxelBatch->removeVoxelFace(b, VOXEL_FACE_FRONT);
voxelBatch->removeVoxelFace(&table[index3D(x, y, z + 1)], VOXEL_FACE_BACK);
}
if (!outOfBounds(x, y, z - 1) && !table[index3D(x, y, z - 1)].isAir())
{
voxelBatch->removeVoxelFace(b, VOXEL_FACE_BACK);
voxelBatch->removeVoxelFace(&table[index3D(x, y, z - 1)], VOXEL_FACE_FRONT);
}
}
}
/*!
* @brief Grabs a snapshot of the map
*/
void getView(char* array, short x, short y)
{
//for the size of the snapshot
for (short index = 0; index < (view.width * view.height); index++)
{
//if the value is out of the bounds of the map it is Void
if ( outOfBounds( (x + index % view.width), (y + index / view.width) ) )
{
array[index] = 'V';
}
//otherwise get the value of the space from the map
else
{
array[index] = currentLevel.map[(x + (y * currentLevel.width)) + (index % view.width) + (index / view.width * currentLevel.width)];
}
}
}
ball_t moveBall(ball_t ball, paddle_t paddle) {
// if the ball hits the left or right edge, reverse the x velocity
if(leftCollision(ball, paddle) || rightCollision(ball)) {
ball.velocity.x *= -1;
}
// if the ball hits the top or bottom edge, reverse the y velocity
if(topCollision(ball) || bottomCollision(ball)) {
ball.velocity.y *= -1;
}
outOfBounds(ball); // check if the ball goes behind the paddle
// makes the ball move
ball.position.x += ball.velocity.x;
ball.position.y += ball.velocity.y;
return ball;
}
/**
* calculates the total of the chi-squared which is minimized
*\para para is the array of parameter values
*/
double fitOM::functND(double*para)
{
//for (int i=0;i<ND;i++) cout << para[i] << " " ;
//cout << endl;
SavePara(para);
React[0]->loadOM();
double out = 0.;
out = React[0]->ChiSquared();
//cout << "chisq= " << out << endl;
//check if a parameter is out of the bounds listed in the *.inp file
if (outOfBounds()) out += 100.;
if (isnan(out))
{
cout << "chisq is nan" << endl;
out = 1e20;
//abort();
}
return out;
}
bool mv_snake(WINDOW *w_game, frame *game, frame *term, snake *snake, int *xPos, int *yPos, char *vector, int cookie[], FILE *urandom, bool vec_diff){
bool mod_length=FALSE;
//save last head position
snake->lastPos_h[0] = *xPos;
snake->lastPos_h[1] = *yPos;
//save last tail pos
snake->lastPos_t[0] = snake->body[0][0];
snake->lastPos_t[1] = snake->body[1][0];
*xPos = snake->body[0][(snake->length)-1] + vector[0];
*yPos = snake->body[1][(snake->length)-1] + vector[1];
if(outOfBounds(xPos, yPos, game)){ //check whether new position is in the field
hitFrame(term, urandom);
}
if(*yPos == cookie[1] && *xPos == cookie[0]){
placeCookie(w_game, game, cookie, urandom);
cookie[0]=-1; //invalidate position
cookie[1]=-1;
(snake->length)++;
mod_length = TRUE;
xPos=&(snake->body[0][(snake->length)-1]); // need to renew position of head in arr temporary
yPos=&(snake->body[1][(snake->length)-1]);
*xPos = snake->lastPos_h[0];
*yPos = snake->lastPos_h[1];
return mod_length;
}
snake->body[0][(snake->length)-2] = snake->lastPos_h[0];
snake->body[1][(snake->length)-2] = snake->lastPos_h[1];
for(int i=0; i<(snake->length)-1; i++){ //move all snake elements one pos left
snake->body[0][i] = snake->body[0][i+1];
snake->body[1][i] = snake->body[1][i+1];
}
return mod_length;
}
void Experiment::odom_cb(const nav_msgs::Odometry msg)
{
geometry_msgs::PoseStamped pose;
odom_msg_ = msg;
if (mode_ != MODE_RETURN)
{
pose.header.stamp = odom_msg_.header.stamp;
pose.header.frame_id = "odom";
pose.pose = odom_msg_.pose.pose;
path_msg_.header.stamp = odom_msg_.header.stamp;
path_msg_.poses.push_back(pose);
path_pub_.publish(path_msg_);
}
else
{
path_msg_.poses.clear();
}
if (outOfBounds() && (mode_ != MODE_RETURN))
stopAndMoveToStart();
}
void Map::remove(int x, int y, int z)
{
voxelBatch->removeVoxel(&table[index3D(x, y, z)]);
if (!outOfBounds(x + 1, y, z) && !table[index3D(x + 1, y, z)].isAir())
voxelBatch->addVoxelFace(&table[index3D(x + 1, y, z)], VOXEL_FACE_LEFT);
if (!outOfBounds(x - 1, y, z) && !table[index3D(x - 1, y, z)].isAir())
voxelBatch->addVoxelFace(&table[index3D(x - 1, y, z)], VOXEL_FACE_RIGHT);
if (!outOfBounds(x, y + 1, z) && !table[index3D(x, y + 1, z)].isAir())
voxelBatch->addVoxelFace(&table[index3D(x, y + 1, z)], VOXEL_FACE_BOTTOM);
if (!outOfBounds(x, y - 1, z) && !table[index3D(x, y - 1, z)].isAir())
voxelBatch->addVoxelFace(&table[index3D(x, y - 1, z)], VOXEL_FACE_TOP);
if (!outOfBounds(x, y, z + 1) && !table[index3D(x, y, z + 1)].isAir())
voxelBatch->addVoxelFace(&table[index3D(x, y, z + 1)], VOXEL_FACE_BACK);
if (!outOfBounds(x, y, z - 1) && !table[index3D(x, y, z - 1)].isAir())
voxelBatch->addVoxelFace(&table[index3D(x, y, z - 1)], VOXEL_FACE_FRONT);
place(AirVoxel(x, y, z));
}
int main(int argc, char *argv[])
{
char szImg[255], szImage[255], buffer[1000], crID[10], szCrList[255], szOut[255];
int ii, kk, size, bigSize=oversampling_factor*srcSize;
int mainlobe_azimuth_min, mainlobe_azimuth_max, mainlobe_range_min;
int mainlobe_range_max, sidelobe_azimuth_min, sidelobe_azimuth_max;
int sidelobe_range_min, sidelobe_range_max, peak_line, peak_sample;
float azimuth_processing_bandwidth, chirp_rate, pulse_duration, sampling_rate;
float prf, srcPeakX, srcPeakY, bigPeakX, bigPeakY, clutter_power, peak_power, scr;
float azimuth_resolution, range_resolution, azimuth_pslr, range_pslr;
float azimuth_window_size, range_window_size;
float azimuth_profile[bigSize], range_profile[bigSize];
static complexFloat *s, *t;
double lat, lon, elev, posX, posY, look_angle;
FILE *fpIn, *fpOut, *fp, *fpText;
meta_parameters *meta, *meta_debug;
float *original_amplitude, *amplitude, *phase;
fcpx *src_fft, *trg_fft;
int debug=FALSE;
char *text=NULL;
int overwrite=FALSE;
if (argc==1) usage();
if (strcmp(argv[1],"-help")==0) help_page(); /* exits program */
if (strcmp(argv[1],"-overwrite")==0) overwrite=TRUE;
int required_args = 4;
if (overwrite) ++required_args;
if(argc != required_args)
usage();/*This exits with a failure*/
/* Fetch required arguments */
strcpy(szImg, argv[argc - 3]);
strcpy(szCrList,argv[argc - 2]);
strcpy(szOut,argv[argc - 1]);
/* DEFAULT VALUES:
size of region to oversample - 64 pixels
mainlobe width factor - 2.6
sidelobe width factor - 20.0
maximum oversampling factor - 8
*/
// Read metadata
sprintf(szImage, "%s.img", szImg);
meta = meta_read(szImage);
lines = meta->general->line_count;
samples = meta->general->sample_count;
text = (char *) MALLOC(255*sizeof(char));
// Handle input and output file
fpIn = FOPEN(szCrList, "r");
fpOut = FOPEN(szOut, "w");
fprintf(fpOut, "POINT TARGET ANALYSIS RESULTS\n\n");
fprintf(fpOut, "CR\tLat\tLon\tElev\tAz peak\tRng peak\tLook\t"
"Az res\tRng res\tAz PSLR\tRng PSLR\tSCR\n");
// RCS needs some more coding
// Loop through corner reflector location file
while (fgets(buffer, 1000, fpIn))
{
if (overwrite) {
sscanf(buffer, "%s\t%lf\t%lf", crID, &posY, &posX);
printf(" %s: posX = %.2lf, posY = %.2lf\n", crID, posX, posY);
}
else {
sscanf(buffer, "%s\t%lf\t%lf\t%lf", crID, &lat, &lon, &elev);
meta_get_lineSamp(meta, lat, lon, elev, &posY, &posX);
printf(" %s: lat = %.4lf, lon = %.4lf, posX = %.2lf, posY = %.2lf\n",
crID, lat, lon, posX, posY);
}
// Check bounds - Get average spectra from chip in range direction
if (!(outOfBounds(posX, posY, srcSize)))
{
// READ SUBSET FROM THE IMAGE WITH CORNER REFLECTOR IN THE CENTER
size = srcSize*srcSize*sizeof(float);
original_amplitude = (float *) MALLOC(size);
phase = (float *) MALLOC(size);
s = (complexFloat *) MALLOC(2*size);
readComplexSubset(szImage, srcSize, srcSize, posX-srcSize/2,
posY-srcSize/2, s);
my_complex2polar(s, srcSize, srcSize, original_amplitude, phase);
if (debug) { // Store original image for debugging
fp = FOPEN("original.img", "wb");
size = bigSize*bigSize*sizeof(float);
FWRITE(original_amplitude, size, 1, fp);
FCLOSE(fp);
meta_debug = meta_init(szImage);
meta_debug->general->line_count =
meta_debug->general->sample_count = srcSize;
meta_debug->general->data_type = REAL32;
meta_debug->general->start_line = posY-srcSize/2;
meta_debug->general->start_sample = posX-srcSize/2;
meta_debug->general->center_latitude = lat;
meta_debug->general->center_longitude = lon;
meta_write(meta_debug, "original.meta");
meta_free(meta_debug);
}
// Find amplitude peak in original image chip
if (!findPeak(original_amplitude, srcSize, &srcPeakX, &srcPeakY)) {
fprintf(fpOut,
" Could not find amplitude peak in original image chip!\n");
goto SKIP;
}
// Cut out the subset again around the peak to make sure we have data for
// the analysis
readComplexSubset(szImage, srcSize, srcSize, posX-srcSize+srcPeakY,
posY-srcSize+srcPeakX, s);
my_complex2polar(s, srcSize, srcSize, original_amplitude, phase);
FREE(phase);
findPeak(original_amplitude, srcSize, &srcPeakX, &srcPeakY);
// Determine look angle
look_angle =
meta_look(meta, srcSize/2, srcSize/2);
/****************************
- special ScanSAR case: images are "projected" - need to be rotated back to
allow analysis in azimuth and range direction (ss_extract.c)
*********************/
// BASEBAND THE DATA IN EACH DIMENSION IN THE FREQUENCY DOMAIN
// Oversample image
src_fft = forward_fft(s, srcSize, srcSize);
trg_fft = oversample(src_fft, srcSize, oversampling_factor);
// Determine azimuth and range window size
azimuth_processing_bandwidth =
(float) meta->sar->azimuth_processing_bandwidth;
prf = (float) meta->sar->prf;
chirp_rate = (float) meta->sar->chirp_rate;
pulse_duration = (float) meta->sar->pulse_duration;
sampling_rate = (float) meta->sar->range_sampling_rate;
azimuth_window_size = azimuth_processing_bandwidth / prf;
range_window_size = fabs(chirp_rate) * pulse_duration / sampling_rate;
printf("azimuth window size: %.2f, range window size: %.2f\n",
azimuth_window_size, range_window_size);
asfRequire(azimuth_window_size > 0.0 && azimuth_window_size < 1.0,
"azimuth window size out of range (0 to 1)!\n");
asfRequire(range_window_size > 0.0 && range_window_size < 1.0,
"range window size out of range (0 to 1)!\n");
if (range_window_size < 0.5)
range_window_size = 0.5;
// for ScanSAR both 0.5
// run debugger to check units are correct!
// Baseband image in range direction
//baseband(src_fft, range_window_size);
/*
// Transpose matrix to work in azimuth direction
//transpose(s);
// Baseband image in azimuth direction
// baseband(s, azimuth_window_size);
// Transpose matrix back into original orientation
//transpose(s);
*/
t = inverse_fft(trg_fft, bigSize, bigSize);
amplitude = (float *) MALLOC(sizeof(float)*bigSize*bigSize);
phase = (float *) MALLOC(sizeof(float)*bigSize*bigSize);
my_complex2polar(t, bigSize, bigSize, amplitude, phase);
FREE(phase);
if (debug) { // Store oversampled image for debugging
fp = FOPEN("oversample.img", "wb");
size = bigSize*bigSize*sizeof(float);
FWRITE(amplitude, size, 1, fp);
FCLOSE(fp);
meta_debug = meta_init("oversample.meta");
meta_debug->general->line_count =
meta_debug->general->sample_count = bigSize;
meta_debug->general->data_type = REAL32;
meta_write(meta_debug, "oversample.meta");
meta_free(meta_debug);
}
// Find the amplitude peak in oversampled image
if (!findPeak(amplitude, bigSize, &bigPeakX, &bigPeakY)) {
fprintf(fpOut,
" Could not find amplitude peak in oversampled image chip!\n");
goto SKIP;
}
peak_line = (int)(bigPeakX + 0.5);
peak_sample = (int)(bigPeakY + 0.5);
// Write text version of oversampled image
sprintf(text, "%s_%s_chip.txt", szImg, crID);
fpText = FOPEN(text, "w");
for (ii=peak_line-32; ii<peak_line+32; ii++) {
for (kk=peak_sample-32; kk<peak_sample+32; kk++)
fprintf(fpText, "%12.4f\t", amplitude[ii*bigSize+kk]);
fprintf(fpText, "\n");
}
FCLOSE(fpText);
// EXTRACTING PROFILES IN AZIMUTH AND RANGE THROUGH PEAK
for (ii=0; ii<bigSize; ii++) {
azimuth_profile[ii] = amplitude[ii*bigSize+peak_sample];
range_profile[ii] = amplitude[bigSize*peak_line+ii];
}
sprintf(text, "%s_%s_azimuth.txt", szImg, crID);
fp = FOPEN(text, "w");
fprintf(fp, "Azimuth profile\n");
for (ii=0; ii<bigSize; ii++)
fprintf(fp, "%.3f\n", azimuth_profile[ii]);
FCLOSE(fp);
sprintf(text, "%s_%s_range.txt", szImg, crID);
fp = FOPEN(text, "w");
fprintf(fp, "Range profile\n");
for (ii=0; ii<bigSize; ii++)
fprintf(fp, "%.3f\n", range_profile[ii]);
FCLOSE(fp);
// FINALLY GET TO THE IMAGE QUALITY PARAMETERS
clutter_power = 0.0;
// Find main lobes in oversampled image
if (!find_mainlobe(amplitude, azimuth_profile, bigSize, peak_line,
clutter_power, &mainlobe_azimuth_min,
&mainlobe_azimuth_max)) {
fprintf(fpOut, " No mainlobes could be found for %s in azimuth!\n",
crID);
goto SKIP;
}
//printf("mainlobe azimuth: min = %d, max = %d\n",
// mainlobe_azimuth_min, mainlobe_azimuth_max);
if (!find_mainlobe(amplitude, range_profile, bigSize, peak_sample,
clutter_power, &mainlobe_range_min,
&mainlobe_range_max)) {
fprintf(fpOut, " No mainlobes could be found for %s in range!\n", crID);
goto SKIP;
}
//printf("mainlobe range: min = %d, max = %d\n",
// mainlobe_range_min, mainlobe_range_max);
// Calculate resolution in azimuth and range for profiles
if (!calc_resolution(azimuth_profile, mainlobe_azimuth_min,
mainlobe_azimuth_max, peak_line,
meta->general->y_pixel_size, clutter_power,
&azimuth_resolution))
fprintf(fpOut, " Negative azimuth resolution for %s - invalid result!"
"\n", crID);
//printf("azimuth resolution = %.2f\n", azimuth_resolution);
if (!calc_resolution(range_profile, mainlobe_range_min,
mainlobe_range_max, peak_sample,
meta->general->x_pixel_size, clutter_power,
&range_resolution))
fprintf(fpOut, " Negative range resolution for %s - invalid result!\n",
crID);
//printf("range resolution = %.2f\n", range_resolution);
// Find peak of original data - thought we had that already: check !!!
// Calculate the clutter power
azimuth_resolution /= meta->general->x_pixel_size;
range_resolution /= meta->general->y_pixel_size;
clutter_power =
calc_clutter_power(original_amplitude, srcSize, peak_sample, peak_line,
azimuth_resolution, range_resolution);
//printf(" Clutter power: %8.3f\n", clutter_power);
// Calculate resolution in azimuth and range with estimated clutter power
if (!calc_resolution(azimuth_profile, mainlobe_azimuth_min,
mainlobe_azimuth_max, peak_line,
meta->general->y_pixel_size, clutter_power,
&azimuth_resolution))
fprintf(fpOut, " Negative azimuth resolution for %s - invalid result!"
"\n", crID);
if (!calc_resolution(range_profile, mainlobe_range_min,
mainlobe_range_max, peak_sample,
meta->general->x_pixel_size, clutter_power,
&range_resolution))
fprintf(fpOut, " Negative range resolution for %s - invalid result!\n",
crID);
//printf(" Azimuth resolution: %.3f\n", azimuth_resolution);
//printf(" Range resolution: %.3f\n", range_resolution);
// Find sidelobes in oversampled image and calculate the point-to-sidelobe
// ratio in azimuth and range direction
if (find_sidelobe(azimuth_profile, bigSize, 1, peak_line,
mainlobe_azimuth_max, &sidelobe_azimuth_max) &&
find_sidelobe(azimuth_profile, bigSize, -1, peak_line,
mainlobe_azimuth_min, &sidelobe_azimuth_min)) {
if (!calc_pslr(azimuth_profile, bigSize, peak_line, sidelobe_azimuth_min,
sidelobe_azimuth_max, &azimuth_pslr))
//printf(" Azimuth PSLR: %.3f\n", azimuth_pslr);
//printf(" No valid PSLR in azimuth could be determined!\n");
;
}
else {
fprintf(fpOut, " Problem in finding sidelobes for %s in azimuth - "
"invalid PSLR!\n", crID);
}
if (find_sidelobe(range_profile, bigSize, 1, peak_sample,
mainlobe_range_max, &sidelobe_range_max) &&
find_sidelobe(range_profile, bigSize, -1, peak_sample,
mainlobe_range_min, &sidelobe_range_min)) {
if (!calc_pslr(range_profile, bigSize, peak_sample, sidelobe_range_min,
sidelobe_range_max, &range_pslr))
//printf(" Range PSLR: %.3f\n", range_pslr);
//printf(" No valid PSLR in range could be determined!\n");
;
}
else {
fprintf(fpOut, " Problem in finding sidelobes for %s in range -"
" invalid PSLR!\n", crID);
}
//printf("sidelobe_azimuth: min = %d, max = %d\n",
// sidelobe_azimuth_min, sidelobe_azimuth_max);
//printf("sidelobe_range: min = %d, max = %d\n",
// sidelobe_range_min, sidelobe_range_max);
// Calculate the signal-to-clutter ratio (SCR)
peak_power = amplitude[peak_line*bigSize+peak_sample] *
amplitude[peak_line*bigSize+peak_sample];
if (clutter_power>0 && peak_power>clutter_power)
scr = 10 * log((peak_power - clutter_power)/clutter_power);
else
scr = 0.0;
if (peak_power > 0.0) {
peak_power = 10 * log(peak_power);
//printf(" Peak power: %.3f\n", peak_power);
//printf(" SCR: %.3f\n", scr);
}
else
fprintf(fpOut, " Negative peak power - invalid result!\n");
FREE(amplitude);
FREE(original_amplitude);
// Write values in output files
fprintf(fpOut, "%s\t%.4lf\t%.4lf\t%.1lf\t%.1lf\t%.1f\t%.3f\t"
"%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n",
crID, lat, lon, elev, posX-srcSize+srcPeakY, posY-srcSize+srcPeakX,
look_angle*R2D, azimuth_resolution, range_resolution, azimuth_pslr,
range_pslr, scr);
SKIP: continue;
}
else
fprintf(fpOut, "\n WARNING: Target %s outside the image boundaries!\n",
crID);
}
FCLOSE(fpIn);
FCLOSE(fpOut);
FREE(meta);
return(0);
}
/**
* Main program.
*/
int main
(int argc,
char **argv)
{
int i, r, c;
long long int t1, t2, t3, t4;
char *infile, *outfile;
FILE *in, *out;
// Start timing.
t1 = currentTimeMillis();
// Parse command line arguments.
if (argc != 3) usage();
infile = argv[1];
outfile = argv[2];
// Read distance matrix from input file.
in = fopen (infile, "r");
if (in == NULL)
{
fprintf (stderr, "Error opening input file \"%s\"\n", infile);
exit (1);
}
n = readInt (in);
d = (double**) malloc (n * sizeof(double*));
for (r = 0; r < n; ++ r)
{
double *d_r = (double*) malloc (n * sizeof(double));
d[r] = d_r;
for (c = 0; c < n; ++ c)
{
d_r[c] = readDouble (in);
}
}
fclose (in);
// Run Floyd's Algorithm.
// for i = 0 to N-1
// for r = 0 to N-1
// for c = 0 to N-1
// D[r,c] = min (D[r,c], D[r,i] + D[i,c])
t2 = currentTimeMillis();
#pragma omp parallel private(i,r,c)
{
for (i = 0; i < n; ++ i)
{
double *d_i;
if (i < 0 || i >= n) outOfBounds();
d_i = d[i];
#pragma omp for schedule(guided)
for (r = 0; r < n; ++ r)
{
double *d_r;
if (r < 0 || r >= n) outOfBounds();
d_r = d[r];
for (c = 0; c < n; ++ c)
{
double d_r_c, d_r_i, d_i_c;
if (c < 0 || c >= n) outOfBounds();
d_r_c = d_r[c];
if (i < 0 || i >= n) outOfBounds();
d_r_i = d_r[i];
if (i < 0 || i >= n) outOfBounds();
d_i_c = d_i[c];
if (c < 0 || c >= n) outOfBounds();
d_r[c] = min (d_r_c, d_r_i + d_i_c);
}
}
}
}
t3 = currentTimeMillis();
// Write distance matrix to output file.
out = fopen (outfile, "w");
if (out == NULL)
{
fprintf (stderr, "Error opening output file \"%s\"\n", outfile);
exit (1);
}
fprintf (out, "%d\n", n);
for (r = 0; r < n; ++ r)
{
double *d_r = d[r];
for (c = 0; c < n; ++ c)
{
fprintf (out, "%e ", d_r[c]);
}
fprintf (out, "\n");
}
fclose (out);
/* Stop timing. */
t4 = currentTimeMillis();
printf ("%lld msec pre\n", t2-t1);
printf ("%lld msec calc\n", t3-t2);
printf ("%lld msec post\n", t4-t3);
printf ("%lld msec total\n", t4-t1);
}
/*!
* @brief check if an x,y coordinate is open for a user to enter
*/
bool const spaceIsOpen(short x, short y)
{
return !outOfBounds(x,y) && spaceTypeOpen(currentLevel.map[ x + (y * currentLevel.width) ]);
}
Image *ImageProcessing::holeFill2(Image *input_image, int max_neighbourhood_size, int min_neighbourhood_size, bool dominant_neighbour) {
Image *img = new Image(*input_image);
bool have_holes = true;
int neighbourhood_size = max_neighbourhood_size;
while (have_holes) {
have_holes = false;
///go through the image and find an empty pixel
for (int y=0;y<img->getHeight();y++) {
for (int x=0;x<img->getWidth();x++) {
///if the pixel is valid then no need to process it
if (img->getPixel(x,y) != Color(0.0f)) continue;
///otherwise compute a value from the local neighbourhood
Vector3f avg_value = Vector3f(0.0f,0.0f,0.0f);
std::vector<Vector3f> neighbour_values;
std::vector<int> neighbour_votes;
int number_of_valid_neighbours = 0;
for (int i=y-1;i<=y+1;i++) {
for (int j=x-1;j<=x+1;j++) {
///check if its out of bounds
if (outOfBounds(img,j,i)) continue;
///if its the pixel in question continue
if (i==y && j==x) continue;
///if the pixel is empty don't count it
if (img->getPixel(j,i)==Color(0.0f)) continue;
///if its substituting by the most dominant neighbour then keep track of the votes
if (dominant_neighbour) {
Vector3f neighbour_value = color2vector3<float>(img->getPixel(j,i));
///first check if the value is already there
bool already_there = false;
int pos = -1;
for (int k=0;k<neighbour_values.size();k++) {
if (neighbour_values[k] == neighbour_value) {
already_there = true;
pos = k;
break;
}
}
///if its already there then increase its votes by 1
if (already_there) {
neighbour_votes[pos]++;
}
else {
///otherwise add it to the neighbour values
neighbour_values.push_back(color2vector3<float>(img->getPixel(j,i)));
///add a single vote
neighbour_votes.push_back(1);
}
}
number_of_valid_neighbours++;
avg_value += color2vector3<float>(img->getPixel(j,i));
}
}
///if there are more than 6 neighbours then fill in the value
if (number_of_valid_neighbours < neighbourhood_size) continue;
if (number_of_valid_neighbours) avg_value /= float(number_of_valid_neighbours);
///if its setting the dominant neighbour then find the one with max votes
if (dominant_neighbour) {
int max_votes = 0;
int pos = -1;
for (int k=0;k<neighbour_votes.size();k++) {
if (neighbour_votes[k] > max_votes) {
max_votes = neighbour_votes[k];
pos = k;
}
}
///set the right value
img->setPixel(x,y,vector2color3(neighbour_values[pos]));
}
else {
img->setPixel(x,y,vector2color3(avg_value));
}
have_holes = true;
}
}
///if there are no changes and its above the min threshold then do it again with a smaller neighbourhood size
if ((!have_holes) && (neighbourhood_size > min_neighbourhood_size)) {
neighbourhood_size-=2;
have_holes = true;
}
}
return img;
}
/* Start of main progam */
int main(int argc, char *argv[])
{
char szOut[255], szImg1[255], szImg2[255], *maskFile;
int x1, x2, y1, y2;
int goodPoints=0, attemptedPoints=0;
FILE *fp_output;
meta_parameters *masterMeta, *slaveMeta;
int logflag=FALSE, ampFlag=TRUE, currArg=1;
while (currArg < (argc-NUM_ARGS)) {
char *key = argv[currArg++];
if (strmatches(key,"-log","--log",NULL)) {
CHECK_ARG(1);
strcpy(logFile,GET_ARG(1));
fLog = FOPEN(logFile, "a");
logflag = TRUE;
}
else if (strmatches(key,"-mask","--mask",NULL)) {
CHECK_ARG(1);
maskFile = GET_ARG(1);
}
else {
printf("\n**Invalid option: %s\n", argv[currArg-1]);
usage(argv[0]);
}
}
if ((argc-currArg) < NUM_ARGS) {
printf("Insufficient arguments.\n");
usage(argv[0]);
}
// Fetch required arguments
strcpy(szImg1,argv[argc - 3]);
strcpy(szImg2,argv[argc - 2]);
strcpy(szOut, argv[argc - 1]);
/* Read metadata */
masterMeta = meta_read(szImg1);
slaveMeta = meta_read(szImg2);
if (masterMeta->general->line_count != slaveMeta->general->line_count ||
masterMeta->general->sample_count != slaveMeta->general->sample_count)
asfPrintWarning("Input images have different dimension!\n");
else if (masterMeta->general->data_type != slaveMeta->general->data_type)
asfPrintError("Input image have different data type!\n");
else if (masterMeta->general->data_type > 5 &&
masterMeta->general->data_type != COMPLEX_REAL32)
asfPrintError("Cannot compare raw images for offsets!\n");
lines = masterMeta->general->line_count;
samples = masterMeta->general->sample_count;
if (masterMeta->general->data_type == COMPLEX_REAL32) {
srcSize = 32;
ampFlag = FALSE;
cZero = Czero();
}
/* Create output file */
fp_output=FOPEN(szOut, "w");
/* Loop over grid, performing forward and backward correlations */
while (getNextPoint(&x1,&y1,&x2,&y2))
{
float dx=0.0, dy=0.0, snr=0.0, dxFW, dyFW, snrFW, dxBW, dyBW, snrBW;
attemptedPoints++;
// Check bounds and mask
if (!(outOfBounds(x1, y1, srcSize, maskFile)))
{
/* ...check forward correlation... */
if (ampFlag) {
if (!(findPeak(x1,y1,szImg1,x2,y2,szImg2,&dxFW,&dyFW,&snrFW))) {
attemptedPoints--;
continue; /* next point if chip in complete background fill */
}
}
else {
getPeak(x1,y1,szImg1,x2,y2,szImg2,&dxFW,&dyFW,&snrFW);
}
if ((!ampFlag && snrFW>minSNR) || (ampFlag)) {
/* ...check backward correlation... */
if (ampFlag) {
if (!(findPeak(x2,y2,szImg2,x1,y1,szImg1,&dxBW,&dyBW,&snrBW))) {
attemptedPoints--;
continue; /* next point if chip in complete background fill */
printf("dxFW: %.2f, dyFW: %.2f\n", dxFW, dyFW);
}
}
else {
getPeak(x2,y2,szImg2,x1,y1,szImg1,&dxBW,&dyBW,&snrBW);
}
dxBW*=-1.0;dyBW*=-1.0;
if (((!ampFlag && snrFW>minSNR) || (ampFlag)) &&
(fabs(dxFW-dxBW) < maxDisp) &&
(fabs(dyFW-dyBW) < maxDisp))
{
dx = (dxFW+dxBW)/2;
dy = (dyFW+dyBW)/2;
snr = snrFW*snrBW;
if (dx < maxDxDy && dy < maxDxDy) goodPoints++;
}
}
fprintf(fp_output,"%6d %6d %8.5f %8.5f %4.2f\n",
x1, y1, x2+dx, y2+dy, snr);
fflush(fp_output);
}
}
if (goodPoints < attemptedPoints)
printf("\n WARNING: %i out of %i points moved by "
"more than one pixel!\n\n",
(attemptedPoints-goodPoints), attemptedPoints);
else
printf("\n There is no difference between the images\n\n");
FCLOSE(fp_output);
FREE(masterMeta);
FREE(slaveMeta);
return(0);
}
void floodfill(){
bool pushingCells = false;
if (wallToTheRight()) {
// && // if there is NO WALL on east
// !liveMaze[xPos][yPos].wallStatus(returnIncrementedFacing())){
setNewWall(returnIncrementedFacing(), xPos, yPos);
pushingCells = true;
// playTone(2000, 200);
}
if (wallToTheLeft()) {
// && // if there is NO WALL on west
// !liveMaze[xPos][yPos].wallStatus(returnDecrementedFacing())){
setNewWall(returnDecrementedFacing(), xPos, yPos);
pushingCells = true;
// playTone(1400, 200);
}
if (wallToTheFront()) {
// && // if there is NO WALL on north
// !liveMaze[xPos][yPos].wallStatus(facing)){
setNewWall(facing, xPos, yPos);
pushingCells = true;
// playTone(500, 200);
}
if(pushingCells)
pushSelfAndAdjacentCells(xPos, yPos);
Serial1.print("xPos: ");
Serial1.println(xPos);
Serial1.print("yPos: ");
Serial1.println(yPos);
while(!floodStack.isEmpty()){
Cell cellCheck = floodStack.pop();
int minDistance = 99;
//Grabbing Minimum Distance
for(int i = 0; i < 4; ++i){
if(!cellCheck.wallStatus(i)){
if(i == 0 && !outOfBounds(cellCheck.returnXCoor() + 1)){
int cellDistance = liveMaze[cellCheck.returnXCoor() + 1]
[cellCheck.returnYCoor()].returnIntDistance();
if(cellDistance <= minDistance)
minDistance = cellDistance;
}
else if(i == 1 && !outOfBounds(cellCheck.returnYCoor() - 1)){
int cellDistance = liveMaze[cellCheck.returnXCoor()]
[cellCheck.returnYCoor() - 1].returnIntDistance();
if(cellDistance <= minDistance)
minDistance = cellDistance;
}
else if(i == 2 && !outOfBounds(cellCheck.returnXCoor() - 1)){
int cellDistance = liveMaze[cellCheck.returnXCoor() - 1]
[cellCheck.returnYCoor()].returnIntDistance();
if(cellDistance <= minDistance)
minDistance = cellDistance;
}
else if(i == 3 && !outOfBounds(cellCheck.returnYCoor() + 1)){
int cellDistance = liveMaze[cellCheck.returnXCoor()]
[cellCheck.returnYCoor() + 1].returnIntDistance();
if(cellDistance <= minDistance)
minDistance = cellDistance;
}
}
}
//The distance of currCell should be the minimum open neighbor + 1
//If not, set that value and push all open neighbors to stack
if(cellCheck.returnIntDistance() != 0 &&
cellCheck.returnIntDistance() != minDistance + 1){
liveMaze[cellCheck.returnXCoor()][cellCheck.returnYCoor()].
setDistance(minDistance + 1);
//Pushing all open neighbors to stack
for(int i = 0; i < 4; ++i){
if(!cellCheck.wallStatus(i)){
if(i == 0 && !outOfBounds(cellCheck.returnXCoor() + 1)){
floodStack.push(liveMaze[cellCheck.returnXCoor() + 1]
[cellCheck.returnYCoor()]);
}
else if(i == 1 && !outOfBounds(cellCheck.returnYCoor() - 1)){
floodStack.push(liveMaze[cellCheck.returnXCoor()]
[cellCheck.returnYCoor() - 1]);
}
else if(i == 2 && !outOfBounds(cellCheck.returnXCoor() - 1)){
floodStack.push(liveMaze[cellCheck.returnXCoor() - 1]
[cellCheck.returnYCoor()]);
}
else if(i == 3 && !outOfBounds(cellCheck.returnYCoor() + 1)){
floodStack.push(liveMaze[cellCheck.returnXCoor()]
[cellCheck.returnYCoor() + 1]);
}
}
}
}
}
}
void GeospatialBoundingBox::computeHeightAndNormalVariation() {
int neighbourhood_size = 3;
int neighbourhood_search_size = neighbourhood_size/2;
///Create a map the same size as the xyz map
if (height_and_normal_variation_map) delete height_and_normal_variation_map;
height_and_normal_variation_map = new Image(xyz_map->getWidth(), xyz_map->getHeight(),0.0f,0.0f,0.0f,0.0f);
///Go through the xyz map and for each point compute the min max values in a 3x3 window area
for (int y=0;y<xyz_map->getHeight();y++) {
for (int x=0;x<xyz_map->getWidth();x++) {
///Check only the valid points
if ((xyz_map->getPixel(x,y) == Color(0.0f,0.0f,0.0f)) || (normal_map->getPixel(x,y) == Color(0.0f,0.0f,0.0f))) continue;
///Get the height of the point in question
float current_height = xyz_map->getPixel(x,y).b();
float min_height = xyz_map->getPixel(x,y).b();
float max_height = xyz_map->getPixel(x,y).b();
float min_dot = 1.0f;
float max_dot = 0.0f;
///Get the normal of the point in question
Vector3f normal = color2vector3(normal_map->getPixel(x,y));
///Search in a neighbourhood
for (int i=y-neighbourhood_search_size;i<=y+neighbourhood_search_size;i++){
for (int j=x-neighbourhood_search_size;j<=x+neighbourhood_search_size;j++) {
///If it's the same continue
if (i==y && j==x) continue;
///Check for out of bounds
if (outOfBounds(xyz_map, j, i)) continue;
///if its a valid point
if (xyz_map->getPixel(j,i) == Color(0.0f,0.0f,0.0f)) continue;
///Check if max or min for the depth
if (min_height > xyz_map->getPixel(j,i)(2)) min_height = xyz_map->getPixel(j,i)(2);
if (max_height < xyz_map->getPixel(j,i)(2)) max_height = xyz_map->getPixel(j,i)(2);
///Check if max or min for the dot product
float dot_product = std::max(0.0f,1.0f - fabs(normal.dot(color2vector3(normal_map->getPixel(j,i)))));
if (min_dot > dot_product) min_dot = dot_product;
if (max_dot < dot_product) max_dot = dot_product;
}
}
///Compute the height variation
float max_minus_min = max_height - min_height;
if (max_minus_min < EPSILON) max_minus_min = 1.0f;
float height_var = (current_height-min_height)/max_minus_min;
///Compute the normal variation
float normal_var = max_dot - min_dot;
///Store it in the variation map
height_and_normal_variation_map->setPixel(x,y,Color(height_var,normal_var,0.0f));
}
}
if (DEBUG) {
height_and_normal_variation_map->saveImage(_format("%s_height_and_dot_variation_map_A.pfm", file_name.c_str()));
}
return;
}
