void ScanResultsWriter::run()
{
std::string nfFilename, plyFilename;
//NeutralFileWriter nfWriter;
PlyWriter plyWriter;
m_cs.enter();
nfFilename = m_nfFilename;
plyFilename = m_plyFilename;
m_cs.leave();
try
{
//nfWriter.open(nfFilename);
plyWriter.begin(plyFilename.c_str());
bool acquiredRecord = false;
NeutralFileRecord record;
//nfWriter.beginBatch();
int numWritten = 0;
while (!Thread::m_stopRequested)
{
// Check to see if there is work to be done
m_cs.enter();
if (!m_records.empty())
{
record = m_records.front();
m_records.pop_front();
acquiredRecord = true;
}
else
{
acquiredRecord = false;
}
m_cs.leave();
// If there is data, do work
if (acquiredRecord)
{
//nfWriter.write(record);
plyWriter.writePoints(&record.point, 1);
numWritten++;
}
else
{
Thread::usleep(1000);
}
// Write to the neutral file in batches
if ((numWritten % 2000) == 0)
{
//nfWriter.commit();
//nfWriter.beginBatch();
}
}
}
catch (Exception& ex)
{
std::cerr << "Error: " << ex << std::endl;
}
catch (...)
{
std::cerr << "!! Unknown error occurred!!!" << std::endl;
}
// Flush any remaining records
//nfWriter.commit();
// Close the output files
//nfWriter.close();
plyWriter.end();
}
void Scanner::run()
{
// Prepare to scan
prepareScan();
// Set the base output file
std::stringstream sstr;
sstr << SCAN_OUTPUT_DIR << std::string("/") << time(NULL);
m_filename = sstr.str();
m_firstRowRightLaserCol = m_camera->getImageWidth() * 0.5;
m_firstRowLeftLaserCol = m_camera->getImageWidth() * 0.5;
Scanner::TimingStats timingStats;
memset(&timingStats, 0, sizeof(Scanner::TimingStats));
timingStats.startTime = GetTimeInSeconds();
double time1 = 0;
Setup * setup = Setup::get();
Preset preset = PresetManager::get()->getActivePreset();
// Read the laser selection
m_laserSelection = preset.laserSide;
// Read the location of the lasers and camera
m_rightLaserLoc = setup->rightLaserLocation;
m_leftLaserLoc = setup->leftLaserLocation;
m_cameraLoc = setup->cameraLocation;
LocationMapper leftLocMapper(m_leftLaserLoc, m_cameraLoc);
LocationMapper rightLocMapper(m_rightLaserLoc, m_cameraLoc);
// Compute the angle between the two laser planes
real leftLaserX = ABS(m_leftLaserLoc.x);
real rightLaserX = ABS(m_rightLaserLoc.x);
real camZ = ABS(m_cameraLoc.z);
// Sanity check to prevent divide by 0. In reality the laser should never be this close to the camera
if (leftLaserX < 0.001)
{
leftLaserX = 0.001;
}
if (rightLaserX < 0.001)
{
rightLaserX = 0.001;
}
//
// tan(theta) = ABS(laserX) / camZ
// theta = atan(ABS(laserX) / camZ)
//
real leftLaserAngle = atan(leftLaserX / camZ);
real rightLaserAngle = atan(rightLaserX / camZ);
m_radiansBetweenLaserPlanes = leftLaserAngle + rightLaserAngle;
// Write the range CSV
if (m_writeRangeCsvEnabled)
{
m_rangeFout.open((m_filename + ".csv").c_str());
if (!m_rangeFout.is_open())
{
throw Exception("Error opening range CSV file");
}
}
// Init the results vectors
m_results.enter();
m_leftLaserResults.clear();
m_rightLaserResults.clear();
m_results.leave();
int numFrames = 0;
int maxFramesPerRevolution = preset.framesPerRevolution;
if (maxFramesPerRevolution < 1)
{
maxFramesPerRevolution = 1;
}
int stepsPerRevolution = Setup::get()->stepsPerRevolution;
if (maxFramesPerRevolution > stepsPerRevolution)
{
maxFramesPerRevolution = stepsPerRevolution;
}
try
{
// Enable the turn table motor
m_turnTable->setMotorEnabled(true);
std::cout << "Enabled motor" << std::endl;
if (m_laser == NULL)
{
throw Exception("Laser object is NULL");
}
if (m_turnTable == NULL)
{
throw Exception("Laser object is NULL");
}
float rotation = 0;
// Read the number of motor steps per revolution
int stepsPerRevolution = setup->stepsPerRevolution;
float rangeRadians = (m_range / 360) * (2 * PI);
// The number of steps for a single frame
int stepsPerFrame = ceil(stepsPerRevolution / (float)maxFramesPerRevolution);
if (stepsPerFrame < 1)
{
stepsPerFrame = 1;
}
// The number of radians a single step takes you
m_radiansPerFrame = ((2 * PI) / (float) stepsPerRevolution);
// The radians to move for a single frame
float frameRadians = stepsPerFrame * m_radiansPerFrame;
numFrames = ceil(rangeRadians / frameRadians);
m_numFramesBetweenLaserPlanes = m_radiansBetweenLaserPlanes / frameRadians;
std::cout << "Angle between laser planes: " << RADIANS_TO_DEGREES(m_radiansBetweenLaserPlanes)
<< " degrees, radiansPerFrame=" << m_radiansPerFrame
<< ", numFramesBetweenLaserPlanes=" << m_numFramesBetweenLaserPlanes
<< ", numFrames=" << numFrames << std::endl;
for (int iFrame = 0; iFrame < numFrames; iFrame++)
{
timingStats.numFrames++;
// Stop if the user asked us to
if (m_stopRequested)
{
break;
}
singleScan(iFrame, rotation, frameRadians, leftLocMapper, rightLocMapper, &timingStats);
rotation += frameRadians;
// Update the progress
double progress = (iFrame + 1.0) / numFrames;
double timeElapsed = GetTimeInSeconds() - m_startTimeSec;
double percentComplete = 100.0 * progress;
double percentPerSecond = percentComplete / timeElapsed;
double fullTimeSec = 100.0 / percentPerSecond;
double remainingSec = fullTimeSec - timeElapsed;
m_progress.setPercent(progress * 100);
m_status.enter();
m_remainingTime = remainingSec;
m_status.leave();
logTimingStats(timingStats);
std::cout << percentComplete << "% Complete, " << (remainingSec / 60) << " minutes remaining." << std::endl;
}
}
catch (...)
{
m_turnTable->setMotorEnabled(false);
m_status.enter();
m_running = false;
m_status.leave();
if (m_writeRangeCsvEnabled)
{
m_rangeFout.close();
}
throw;
}
m_rangeFout.close();
m_turnTable->setMotorEnabled(false);
std::cout << "Merging laser results..." << std::endl;
time1 = GetTimeInSeconds();
// Merge the left and right lasers and sort the results
std::vector<NeutralFileRecord> results;
LaserResultsMerger merger;
m_results.enter();
merger.merge(results, m_leftLaserResults, m_rightLaserResults, maxFramesPerRevolution,
m_numFramesBetweenLaserPlanes, Camera::getInstance()->getImageHeight(), preset.laserMergeAction, m_progress);
m_results.leave();
// Sort by pseudo-step and row
std::cout << "Sort 2... " << std::endl;
std::sort(results.begin(), results.end(), ComparePseudoSteps);
std::cout << "End Sort 2... " << std::endl;
m_results.enter();
std::cout << "Merged " << m_leftLaserResults.size() << " left laser and " << m_rightLaserResults.size() << " right laser results into " << results.size() << " results." << std::endl;
m_leftLaserResults.clear();
m_rightLaserResults.clear();
m_results.leave();
std::cout << "Constructing mesh..." << std::endl;
timingStats.laserMergeTime += GetTimeInSeconds() - time1;
// Write the PLY file
std::cout << "Starting output thread..." << std::endl;
if (preset.generatePly)
{
m_progress.setLabel("Generating PLY file");
m_progress.setPercent(0);
std::string plyFilename = m_filename + ".ply";
std::cout << "Writing PLY file... " << plyFilename << std::endl;
time1 = GetTimeInSeconds();
FileWriter plyOut(plyFilename.c_str());
if (!plyOut.is_open())
{
throw Exception("Error opening file for writing: " + plyFilename);
}
/** Writes the results to a PLY file */
PlyWriter plyWriter;
plyWriter.setDataFormat(preset.plyDataFormat);
plyWriter.setTotalNumPoints((int)results.size());
plyWriter.begin(&plyOut);
real percent = 0;
for (size_t iRec = 0; iRec < results.size(); iRec++)
{
real newPct = 100.0f * iRec / results.size();
if (newPct - percent > 0.1)
{
m_progress.setPercent(newPct);
percent = newPct;
}
plyWriter.writePoints(&results[iRec].point, 1);
}
plyWriter.end();
plyOut.close();
timingStats.pointCloudWritingTime += GetTimeInSeconds() - time1;
}
// Generate the XYZ file
if (preset.generateXyz)
{
std::cout << "Generating XYZ file..." << std::endl;
time1 = GetTimeInSeconds();
XyzWriter xyzWriter;
xyzWriter.write(m_filename, results, m_progress);
timingStats.pointCloudWritingTime += GetTimeInSeconds() - time1;
}
// Generate the STL file
if (preset.generateStl)
{
std::cout << "Generating STL mesh..." << std::endl;
time1 = GetTimeInSeconds();
StlWriter stlWriter;
stlWriter.write(m_filename, results, m_range > 359, m_progress);
timingStats.meshWritingTime = GetTimeInSeconds() - time1;
}
logTimingStats(timingStats);
m_progress.setPercent(100);
m_status.enter();
m_running = false;
m_status.leave();
std::cout << "Done." << std::endl;
}
