void RangeScanner::image_sensor() {
if(false) {
sparse_scan = SparseRangeScan("dense_scan.scan");
return;
}
ImplicitNewton* implicit_solver = (lipschitz == 0) ? new ImplicitNewton(shape, 1.0, 1e-7) : new ImplicitNewton(shape, 1.0, 1e-7, lipschitz);
double my_pi = acos(-1);
int total_res = sensor.resx()*sensor.resy();
int fraction = total_res/10;
int ind = 0;
int num_hits = 0, num_valid_hits = 0;
Vector3 ray_pos, ray_dir;
double t_hit;
for(int y = 0; y < sensor.resy(); y++) {
for(int x = 0; x < sensor.resx(); x++) {
ind++;
if((ind % fraction) == 0)
cout << ((double)ind / (double)total_res) << " done with range image... " << num_valid_hits << " / " << num_hits << endl;
sensor.generatePerspectiveRay(x, y, ray_pos, ray_dir);
if(implicit_solver->intersection(ray_pos, ray_dir, t_hit)) {
num_hits++;
Vector3 range_pt = ray_pos + ray_dir*t_hit;
Vector3 laser_dir = range_pt-laser_pos;
laser_dir.normalize();
double laser_t;
if(!implicit_solver->intersection(laser_pos, laser_dir, laser_t)) {
image_scan.addPoint(PixelCoord(x,y), range_pt);
continue;
}
Vector3 laser_strike = laser_pos + laser_dir*laser_t;
double laser_diff = laser_strike.length(range_pt);
if(laser_diff > 1e-2) {
image_scan.addPoint(PixelCoord(x,y), range_pt);
continue;
}
num_valid_hits++;
sparse_scan.addPoint(PixelCoord(x,y), range_pt);
}
}
}
delete implicit_solver;
//sparse_scan.write_to_file("dense_scan.scan");
}
///////////////////////////////////////////////////////////////////////////////
// calculates and returns the input pixel coordinates of the outut
// coordinates xOut / yOut
PixelCoord PixToPix::CalcInPixels(double xOut, double yOut)
{
double xPos, yPos; // sky coordinates
double xIn, yIn;
int offscl = 0;
pix2wcs(m_outWcs, xOut, yOut, &xPos, &yPos);
gal2fk5(&xPos, &yPos);
wcs2pix(m_inWcs, xPos, yPos, &xIn, &yIn, &offscl);
return PixelCoord(xIn, yIn, true);
}
///////////////////////////////////////////////////////////////////////////////
// calculates and returns the output pixel coordinates of the input
// coordinates xIn / yIn
// PixelCoord.m_valid is set to false if the coordinates outside of the
// of the output area
PixelCoord PixToPix::CalcOutPixels(double xIn, double yIn)
{
double xPos, yPos; // sky coordinates
double xOut, yOut;
bool valid = true;
int offscl = 0;
/* Pixels in fits images don't start at 0, but 1. C images start at 0...*/
pix2wcs(m_inWcs, xIn, yIn, &xPos, &yPos);
/* Now, xPos and yPos are in equatorial (ie, RA, DEC).
This is because this is what our input is in, ie, if you have something not
integral this won't work */
/* Convert to Glactic */
fk52gal(&xPos,&yPos);
// test if we are inside the current sky area
if (yPos < m_lowB || yPos > m_highB)
valid = false;
if (m_lowL < m_highL)
{
// normal case
if (xPos < m_lowL || xPos > m_highL)
valid = false;
}
else
{
// we crossed the 0/360 line
if (xPos < m_lowL && xPos > m_highL)
valid = false;
}
if (valid)
{
wcs2pix(m_outWcs, xPos, yPos, &xOut, &yOut, &offscl);
if (offscl) valid = false;
}
return PixelCoord(xOut, yOut, valid);
}
