int CVxParamArray::ReadFrame(int frameNumber)
{
// check if user specified input file to read from
if (m_fileNameRead.length() < 1) return 0;
// for single frame reads, there is no need to read the array again
// as it is already read into the object
if (!m_fileNameForReadHasIndex && frameNumber != m_captureFrameStart) {
return 0;
}
// reading data from input file
char fileName[MAX_FILE_NAME_LENGTH]; sprintf(fileName, m_fileNameRead.c_str(), frameNumber);
FILE * fp = fopen(fileName, m_readFileIsBinary ? "rb" : "r");
if(!fp) {
if (frameNumber >= (int)m_captureFrameStart) {
// end of sequence detected for multiframe sequences
return 1;
}
else ReportError("ERROR: Unable to open: %s\n", fileName);
}
size_t numItems = ReadFileIntoBuffer(fp, m_readFileIsBinary);
fclose(fp);
// set array size to numItems and write the data into array object
ERROR_CHECK(vxTruncateArray(m_array, 0));
if (numItems > 0) {
ERROR_CHECK(vxAddArrayItems(m_array, numItems, m_bufForRead, m_itemSize));
}
// process user requested directives
if (m_useSyncOpenCLWriteDirective) {
ERROR_CHECK_AND_WARN(vxDirective((vx_reference)m_array, VX_DIRECTIVE_AMD_COPY_TO_OPENCL), VX_ERROR_NOT_ALLOCATED);
}
return 0;
}
int main(int argc, char* argv[])
{
try
{
nvxio::Application &app = nvxio::Application::get();
//
// Parse command line arguments
//
// std::string sourceUri = app.findSampleFilePath("file:///dev/video0");
// "/home/ubuntu/VisionWorks-SFM-0.82-Samples/data/sfm/parking_sfm.mp4";
std::string sourceUri = "/home/px4/test.mp4";
std::string configFile = app.findSampleFilePath("sfm/sfm_config.ini");
bool fullPipeline = false;
std::string maskFile;
bool noLoop = false;
app.setDescription("This sample demonstrates Structure from Motion (SfM) algorithm");
app.addOption(0, "mask", "Optional mask", nvxio::OptionHandler::string(&maskFile));
app.addBooleanOption('f', "fullPipeline", "Run full SfM pipeline without using IMU data", &fullPipeline);
app.addBooleanOption('n', "noLoop", "Run sample without loop", &noLoop);
app.init(argc, argv);
nvx_module_version_t sfmVersion;
nvxSfmGetVersion(&sfmVersion);
std::cout << "VisionWorks SFM version: " << sfmVersion.major << "." << sfmVersion.minor
<< "." << sfmVersion.patch << sfmVersion.suffix << std::endl;
std::string imuDataFile;
std::string frameDataFile;
if (!fullPipeline)
{
imuDataFile = app.findSampleFilePath("sfm/imu_data.txt");
frameDataFile = app.findSampleFilePath("sfm/images_timestamps.txt");
}
if (app.getPreferredRenderName() != "default")
{
std::cerr << "The sample uses custom Render for GUI. --nvxio_render option is not supported!" << std::endl;
return nvxio::Application::APP_EXIT_CODE_NO_RENDER;
}
//
// Read SfMParams
//
nvx::SfM::SfMParams params;
std::string msg;
if (!read(configFile, params, msg))
{
std::cout << msg << std::endl;
return nvxio::Application::APP_EXIT_CODE_INVALID_VALUE;
}
//
// Create OpenVX context
//
nvxio::ContextGuard context;
//
// Messages generated by the OpenVX framework will be processed by nvxio::stdoutLogCallback
//
vxRegisterLogCallback(context, &nvxio::stdoutLogCallback, vx_false_e);
//
// Add SfM kernels
//
NVXIO_SAFE_CALL(nvxSfmRegisterKernels(context));
//
// Create a Frame Source
//
std::unique_ptr<nvxio::FrameSource> source(
nvxio::createDefaultFrameSource(context, sourceUri));
if (!source || !source->open())
{
std::cout << "Can't open source file: " << sourceUri << std::endl;
// int haha=3;
// fprintf(stderr, "errno = %d \n", haha);
return nvxio::Application::APP_EXIT_CODE_NO_RESOURCE;
}
nvxio::FrameSource::Parameters sourceParams = source->getConfiguration();
//
// Create OpenVX Image to hold frames from video source
//
vx_image frame = vxCreateImage(context,
sourceParams.frameWidth, sourceParams.frameHeight, sourceParams.format);
NVXIO_CHECK_REFERENCE(frame);
//
// Load mask image if needed
//
vx_image mask = NULL;
if (!maskFile.empty())
{
mask = nvxio::loadImageFromFile(context, maskFile, VX_DF_IMAGE_U8);
vx_uint32 mask_width = 0, mask_height = 0;
vxQueryImage(mask, VX_IMAGE_ATTRIBUTE_WIDTH, &mask_width, sizeof(mask_width));
vxQueryImage(mask, VX_IMAGE_ATTRIBUTE_HEIGHT, &mask_height, sizeof(mask_height));
if (mask_width != sourceParams.frameWidth || mask_height != sourceParams.frameHeight)
{
std::cerr << "The mask must have the same size as the input source." << std::endl;
return nvxio::Application::APP_EXIT_CODE_INVALID_DIMENSIONS;
}
}
//
// Create 3D Render instance
//
std::unique_ptr<nvxio::Render3D> render3D(nvxio::createDefaultRender3D(context, 0, 0,
"SfM Point Cloud", sourceParams.frameWidth, sourceParams.frameHeight));
nvxio::Render::TextBoxStyle style = {{255, 255, 255, 255}, {0, 0, 0, 255}, {10, 10}};
if (!render3D)
{
std::cerr << "Can't create a renderer" << std::endl;
return nvxio::Application::APP_EXIT_CODE_NO_RENDER;
}
float fovYinRad = 2.f * atanf(sourceParams.frameHeight / 2.f / params.pFy);
render3D->setDefaultFOV(180.f / nvxio::PI_F * fovYinRad);
EventData eventData;
render3D->setOnKeyboardEventCallback(eventCallback, &eventData);
//
// Create SfM class instance
//
std::unique_ptr<nvx::SfM> sfm(nvx::SfM::createSfM(context, params));
//
// Create FenceDetectorWithKF class instance
//
FenceDetectorWithKF fenceDetector;
nvxio::FrameSource::FrameStatus frameStatus;
do
{
frameStatus = source->fetch(frame);
}
while (frameStatus == nvxio::FrameSource::TIMEOUT);
if (frameStatus == nvxio::FrameSource::CLOSED)
{
std::cerr << "Source has no frames" << std::endl;
return nvxio::Application::APP_EXIT_CODE_NO_FRAMESOURCE;
}
vx_status status = sfm->init(frame, mask, imuDataFile, frameDataFile);
if (status != VX_SUCCESS)
{
std::cerr << "Failed to initialize the algorithm" << std::endl;
return nvxio::Application::APP_EXIT_CODE_ERROR;
}
const vx_size maxNumOfPoints = 2000;
const vx_size maxNumOfPlanesVertices = 2000;
vx_array filteredPoints = vxCreateArray(context, NVX_TYPE_POINT3F, maxNumOfPoints);
vx_array planesVertices = vxCreateArray(context, NVX_TYPE_POINT3F, maxNumOfPlanesVertices);
//
// Run processing loop
//
vx_matrix model = vxCreateMatrix(context, VX_TYPE_FLOAT32, 4, 4);
float eye_data[4*4] = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};
vxWriteMatrix(model, eye_data);
nvxio::Render3D::PointCloudStyle pcStyle = {0, 12};
nvxio::Render3D::PlaneStyle fStyle = {0, 10};
GroundPlaneSmoother groundPlaneSmoother(7);
nvx::Timer totalTimer;
totalTimer.tic();
double proc_ms = 0;
float yGroundPlane = 0;
while (!eventData.shouldStop)
{
if (!eventData.pause)
{
frameStatus = source->fetch(frame);
if (frameStatus == nvxio::FrameSource::TIMEOUT)
{
continue;
}
if (frameStatus == nvxio::FrameSource::CLOSED)
{
if(noLoop) break;
if (!source->open())
{
std::cerr << "Failed to reopen the source" << std::endl;
break;
}
do
{
frameStatus = source->fetch(frame);
}
while (frameStatus == nvxio::FrameSource::TIMEOUT);
sfm->init(frame, mask, imuDataFile, frameDataFile);
fenceDetector.reset();
continue;
}
// Process
nvx::Timer procTimer;
procTimer.tic();
sfm->track(frame, mask);
proc_ms = procTimer.toc();
}
// Print performance results
sfm->printPerfs();
if (!eventData.showPointCloud)
{
render3D->disableDefaultKeyboardEventCallback();
render3D->putImage(frame);
}
else
{
render3D->enableDefaultKeyboardEventCallback();
}
filterPoints(sfm->getPointCloud(), filteredPoints);
render3D->putPointCloud(filteredPoints, model, pcStyle);
if (eventData.showFences)
{
fenceDetector.getFencePlaneVertices(filteredPoints, planesVertices);
render3D->putPlanes(planesVertices, model, fStyle);
}
if (fullPipeline && eventData.showGP)
{
const float x1(-1.5), x2(1.5), z1(1), z2(4);
vx_matrix gp = sfm->getGroundPlane();
yGroundPlane = groundPlaneSmoother.getSmoothedY(gp, x1, z1);
nvx_point3f_t pt[4] = {{x1, yGroundPlane, z1},
{x1, yGroundPlane, z2},
{x2, yGroundPlane, z2},
{x2, yGroundPlane, z1}};
vx_array gpPoints = vxCreateArray(context, NVX_TYPE_POINT3F, 4);
vxAddArrayItems(gpPoints, 4, pt, sizeof(pt[0]));
render3D->putPlanes(gpPoints, model, fStyle);
vxReleaseArray(&gpPoints);
}
double total_ms = totalTimer.toc();
// Add a delay to limit frame rate
app.sleepToLimitFPS(total_ms);
total_ms = totalTimer.toc();
totalTimer.tic();
std::string state = createInfo(fullPipeline, proc_ms, total_ms, eventData);
render3D->putText(state.c_str(), style);
if (!render3D->flush())
{
eventData.shouldStop = true;
}
}
//
// Release all objects
//
vxReleaseImage(&frame);
vxReleaseImage(&mask);
vxReleaseMatrix(&model);
vxReleaseArray(&filteredPoints);
vxReleaseArray(&planesVertices);
}
catch (const std::exception& e)
{
std::cerr << "Error: " << e.what() << std::endl;
return nvxio::Application::APP_EXIT_CODE_ERROR;
}
return nvxio::Application::APP_EXIT_CODE_SUCCESS;
}
// nodeless version of the Fast9Corners kernel
vx_status vxFast9Corners(vx_image src, vx_scalar sens, vx_scalar nonm, vx_array points,
vx_scalar s_num_corners, vx_border_mode_t *bordermode)
{
vx_float32 b = 0.0f;
vx_imagepatch_addressing_t src_addr;
void *src_base = NULL;
vx_rectangle_t rect;
vx_uint8 tolerance = 0;
vx_bool do_nonmax;
vx_uint32 num_corners = 0;
vx_size dst_capacity = 0;
vx_keypoint_t kp;
vx_status status = vxGetValidRegionImage(src, &rect);
status |= vxReadScalarValue(sens, &b);
status |= vxReadScalarValue(nonm, &do_nonmax);
/* remove any pre-existing points */
status |= vxTruncateArray(points, 0);
status |= vxAccessImagePatch(src, &rect, 0, &src_addr, &src_base, VX_READ_ONLY);
tolerance = (vx_uint8)b;
status |= vxQueryArray(points, VX_ARRAY_ATTRIBUTE_CAPACITY, &dst_capacity, sizeof(dst_capacity));
memset(&kp, 0, sizeof(kp));
if (status == VX_SUCCESS)
{
/*! \todo implement other Fast9 Corners border modes */
if (bordermode->mode == VX_BORDER_MODE_UNDEFINED)
{
vx_int32 y, x;
for (y = APERTURE; y < (vx_int32)(src_addr.dim_y - APERTURE); y++)
{
for (x = APERTURE; x < (vx_int32)(src_addr.dim_x - APERTURE); x++)
{
vx_uint8 strength = vxGetFastCornerStrength(x, y, src_base, &src_addr, tolerance);
if (strength > 0)
{
if (do_nonmax)
{
if (strength >= vxGetFastCornerStrength(x-1, y-1, src_base, &src_addr, tolerance) &&
strength >= vxGetFastCornerStrength(x, y-1, src_base, &src_addr, tolerance) &&
strength >= vxGetFastCornerStrength(x+1, y-1, src_base, &src_addr, tolerance) &&
strength >= vxGetFastCornerStrength(x-1, y, src_base, &src_addr, tolerance) &&
strength > vxGetFastCornerStrength(x+1, y, src_base, &src_addr, tolerance) &&
strength > vxGetFastCornerStrength(x-1, y+1, src_base, &src_addr, tolerance) &&
strength > vxGetFastCornerStrength(x, y+1, src_base, &src_addr, tolerance) &&
strength > vxGetFastCornerStrength(x+1, y+1, src_base, &src_addr, tolerance))
;
else
continue;
}
if (num_corners < dst_capacity)
{
kp.x = x;
kp.y = y;
kp.strength = strength;
status |= vxAddArrayItems(points, 1, &kp, 0);
}
num_corners++;
}
}
}
}
else
{
status = VX_ERROR_NOT_IMPLEMENTED;
}
if (s_num_corners)
status |= vxWriteScalarValue(s_num_corners, &num_corners);
status |= vxCommitImagePatch(src, NULL, 0, &src_addr, src_base);
}
return status;
}
