void Scanner::generateDebugInfo(Laser::LaserSide laserSide) { // Prepare for scanning prepareScan(); m_laser->turnOff(Laser::ALL_LASERS); acquireImage(&m_image1); m_laser->turnOn(laserSide); acquireImage(&m_image2); m_laser->turnOff(laserSide); Image debuggingImage; std::string debuggingCsv = std::string(DEBUG_OUTPUT_DIR) + "/0.csv"; int firstRowLaserCol = m_camera->getImageWidth() * 0.5; real percentPixelsOverThreshold = 0; int numLocations = m_imageProcessor.process(m_image1, m_image2, &debuggingImage, m_laserLocations, m_maxNumLocations, firstRowLaserCol, percentPixelsOverThreshold, debuggingCsv.c_str()); std::string baseFilename = std::string(DEBUG_OUTPUT_DIR) + "/"; // Overlay the pixels onto the debug image and write that as a new image PixelLocationWriter locWriter; Image::overlayPixels(debuggingImage, m_laserLocations, numLocations); locWriter.writeImage(debuggingImage, debuggingImage.getWidth(), debuggingImage.getHeight(), baseFilename + "5.png"); m_progress.setPercent(100); m_status.enter(); m_running = false; m_status.leave(); std::cout << "Done." << std::endl; }
Filesystem::Filesystem() { connect(&m_watcher, SIGNAL(directoryChanged(QString)), SLOT(prepareScan(QString))); }
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; }