static vx_status VX_CALLBACK vxCopyArrayOutputValidator(vx_node node, vx_uint32 index, vx_meta_format meta)
{
vx_status status = VX_ERROR_INVALID_PARAMETERS;
if (index > 0)
{
vx_parameter param = vxGetParameterByIndex(node, 0);
if (param)
{
vx_array input;
vxQueryParameter(param, VX_PARAMETER_ATTRIBUTE_REF, &input, sizeof(vx_array));
if (input)
{
vx_enum item_type = VX_TYPE_INVALID;
vx_size capacity = 0ul;
status = VX_SUCCESS;
status |= vxQueryArray(input, VX_ARRAY_ATTRIBUTE_ITEMTYPE, &item_type, sizeof(item_type));
status |= vxQueryArray(input, VX_ARRAY_ATTRIBUTE_CAPACITY, &capacity, sizeof(capacity));
status |= vxSetMetaFormatAttribute(meta, VX_ARRAY_ATTRIBUTE_ITEMTYPE, &item_type, sizeof(item_type));
status |= vxSetMetaFormatAttribute(meta, VX_ARRAY_ATTRIBUTE_CAPACITY, &capacity, sizeof(capacity));
vxReleaseArray(&input);
}
vxReleaseParameter(¶m);
}
}
return status;
}
static vx_status VX_CALLBACK vxFindWarpInputValidator(vx_node node, vx_uint32 index)
{
vx_status status = VX_ERROR_INVALID_PARAMETERS;
if (index == 0 || index == 1 )
{
vx_parameter param1 = vxGetParameterByIndex(node, 0);
vx_parameter param2 = vxGetParameterByIndex(node, 1);
if (param1 && param2)
{
vx_array arr1 = 0, arr2 = 0;
vxQueryParameter(param1, VX_PARAMETER_ATTRIBUTE_REF, &arr1, sizeof(arr1));
vxQueryParameter(param2, VX_PARAMETER_ATTRIBUTE_REF, &arr2, sizeof(arr2));
if (arr1 && arr2)
{
vx_enum item_type1 = 0, item_type2 = 0;
vxQueryArray(arr1, VX_ARRAY_ATTRIBUTE_ITEMTYPE, &item_type1, sizeof(item_type1));
vxQueryArray(arr2, VX_ARRAY_ATTRIBUTE_ITEMTYPE, &item_type2, sizeof(item_type2));
if (item_type1 == VX_TYPE_KEYPOINT && item_type2 == VX_TYPE_KEYPOINT)
{
vx_size num1, num2;
vxQueryArray(arr1, VX_ARRAY_ATTRIBUTE_NUMITEMS, &num1, sizeof(num1));
vxQueryArray(arr2, VX_ARRAY_ATTRIBUTE_NUMITEMS, &num2, sizeof(num2));
if(num1 == num2)
{
status = VX_SUCCESS;
}
}
vxReleaseArray(&arr1);
vxReleaseArray(&arr2);
}
vxReleaseParameter(¶m1);
vxReleaseParameter(¶m2);
}
}
return status;
}
int CVxParamArray::Shutdown(void)
{
if (m_compareCountMatches > 0 && m_compareCountMismatches == 0) {
printf("OK: array COMPARE MATCHED for %d frame(s) of %s\n", m_compareCountMatches, GetVxObjectName());
}
if (m_array) {
vxReleaseArray(&m_array);
m_array = nullptr;
}
if (m_bufForRead) {
delete[] m_bufForRead;
m_bufForRead = nullptr;
}
return 0;
}
int CVxParamArray::Initialize(vx_context context, vx_graph graph, const char * desc)
{
// get object parameters and create object
char objType[64];
const char * ioParams = ScanParameters(desc, "array|virtual-array:", "s:", objType);
if (!_stricmp(objType, "array") || !_stricmp(objType, "virtual-array") ||
!_stricmp(objType, "array-virtual"))
{
// syntax: [virtual-]array:<format>,<capacity>[:<io-params>]
char itemType[64];
ioParams = ScanParameters(ioParams, "<format>,<capacity>", "s,D", &itemType, &m_capacity);
bool found_userStruct = false;
for (auto it = m_userStructMap->begin(); it != m_userStructMap->end(); ++it){
if (strcmp(itemType, it->first.c_str()) == 0){
found_userStruct = true;
m_format = it->second;
}
}
if (found_userStruct == false){
m_format = ovxName2Enum(itemType);
if (m_format == 0) {
ReportError("ERROR: invalid array item type specified: %s\n", itemType);
}
}
// create array object
if (!_stricmp(objType, "virtual-array") || !_stricmp(objType, "array-virtual")) {
m_array = vxCreateVirtualArray(graph, m_format, m_capacity);
m_isVirtualObject = true;
}
else {
m_array = vxCreateArray(context, m_format, m_capacity);
}
}
else ReportError("ERROR: unsupported array type: %s\n", desc);
vx_status ovxStatus = vxGetStatus((vx_reference)m_array);
if (ovxStatus != VX_SUCCESS){
printf("ERROR: array creation failed => %d (%s)\n", ovxStatus, ovxEnum2Name(ovxStatus));
if (m_array) vxReleaseArray(&m_array);
throw - 1;
}
m_vxObjRef = (vx_reference)m_array;
// io initialize
return InitializeIO(context, graph, m_vxObjRef, ioParams);
}
/*!***********************************************************************************************************
output parameter validator.
*************************************************************************************************************/
static vx_status VX_CALLBACK CV_SURF_Compute_OutputValidator(vx_node node, vx_uint32 index, vx_meta_format meta)
{
vx_status status = VX_SUCCESS;
if (index == 2)
{
vx_parameter output_param = vxGetParameterByIndex(node, 2);
vx_array output; vx_size size = 0;
STATUS_ERROR_CHECK(vxQueryParameter(output_param, VX_PARAMETER_ATTRIBUTE_REF, &output, sizeof(vx_array)));
STATUS_ERROR_CHECK(vxQueryArray(output, VX_ARRAY_ATTRIBUTE_CAPACITY, &size, sizeof(size)));
if (size <= 0)
status = VX_ERROR_INVALID_VALUE;
vxReleaseArray(&output);
vxReleaseParameter(&output_param);
}
return status;
}
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;
}
/*!***********************************************************************************************************
input parameter validator.
param [in] node The handle to the node.
param [in] index The index of the parameter to validate.
*************************************************************************************************************/
static vx_status VX_CALLBACK CV_SURF_Compute_InputValidator(vx_node node, vx_uint32 index)
{
vx_status status = VX_SUCCESS;
vx_parameter param = vxGetParameterByIndex(node, index);
if (index == 0)
{
vx_image image;
vx_df_image df_image = VX_DF_IMAGE_VIRT;
STATUS_ERROR_CHECK(vxQueryParameter(param, VX_PARAMETER_ATTRIBUTE_REF, &image, sizeof(vx_image)));
STATUS_ERROR_CHECK(vxQueryImage(image, VX_IMAGE_ATTRIBUTE_FORMAT, &df_image, sizeof(df_image)));
if (df_image != VX_DF_IMAGE_U8)
status = VX_ERROR_INVALID_VALUE;
vxReleaseImage(&image);
}
if (index == 1)
{
vx_image image;
vx_df_image df_image = VX_DF_IMAGE_VIRT;
STATUS_ERROR_CHECK(vxQueryParameter(param, VX_PARAMETER_ATTRIBUTE_REF, &image, sizeof(vx_image)));
STATUS_ERROR_CHECK(vxQueryImage(image, VX_IMAGE_ATTRIBUTE_FORMAT, &df_image, sizeof(df_image)));
if (df_image != VX_DF_IMAGE_U8)
status = VX_ERROR_INVALID_VALUE;
vxReleaseImage(&image);
}
else if (index == 2)
{
vx_array array; vx_size size = 0;
STATUS_ERROR_CHECK(vxQueryParameter(param, VX_PARAMETER_ATTRIBUTE_REF, &array, sizeof(array)));
STATUS_ERROR_CHECK(vxQueryArray(array, VX_ARRAY_ATTRIBUTE_CAPACITY, &size, sizeof(size)));
vxReleaseArray(&array);
}
else if (index == 3)
{
vx_array array; vx_size size = 0;
STATUS_ERROR_CHECK(vxQueryParameter(param, VX_PARAMETER_ATTRIBUTE_REF, &array, sizeof(array)));
STATUS_ERROR_CHECK(vxQueryArray(array, VX_ARRAY_ATTRIBUTE_CAPACITY, &size, sizeof(size)));
vxReleaseArray(&array);
}
else if (index == 4)
{
vx_scalar scalar = 0; vx_enum type = 0; vx_float32 value = 0;
STATUS_ERROR_CHECK(vxQueryParameter(param, VX_PARAMETER_ATTRIBUTE_REF, &scalar, sizeof(scalar)));
STATUS_ERROR_CHECK(vxQueryScalar(scalar, VX_SCALAR_ATTRIBUTE_TYPE, &type, sizeof(type)));
STATUS_ERROR_CHECK(vxReadScalarValue(scalar, &value));
if (value < 0 || type != VX_TYPE_FLOAT32)
status = VX_ERROR_INVALID_VALUE;
vxReleaseScalar(&scalar);
}
else if (index == 5)
{
vx_scalar scalar = 0; vx_enum type = 0; vx_int32 value = 0;
STATUS_ERROR_CHECK(vxQueryParameter(param, VX_PARAMETER_ATTRIBUTE_REF, &scalar, sizeof(scalar)));
STATUS_ERROR_CHECK(vxQueryScalar(scalar, VX_SCALAR_ATTRIBUTE_TYPE, &type, sizeof(type)));
STATUS_ERROR_CHECK(vxReadScalarValue(scalar, &value));
if (value < 0 || type != VX_TYPE_INT32)
status = VX_ERROR_INVALID_VALUE;
vxReleaseScalar(&scalar);
}
else if (index == 6)
{
vx_scalar scalar = 0; vx_enum type = 0; vx_int32 value = 0;
STATUS_ERROR_CHECK(vxQueryParameter(param, VX_PARAMETER_ATTRIBUTE_REF, &scalar, sizeof(scalar)));
STATUS_ERROR_CHECK(vxQueryScalar(scalar, VX_SCALAR_ATTRIBUTE_TYPE, &type, sizeof(type)));
STATUS_ERROR_CHECK(vxReadScalarValue(scalar, &value));
if (value < 0 || type != VX_TYPE_INT32)
status = VX_ERROR_INVALID_VALUE;
vxReleaseScalar(&scalar);
}
else if (index == 7)
{
vx_scalar scalar = 0; vx_enum type = 0; vx_bool value = vx_true_e;
STATUS_ERROR_CHECK(vxQueryParameter(param, VX_PARAMETER_ATTRIBUTE_REF, &scalar, sizeof(scalar)));
STATUS_ERROR_CHECK(vxQueryScalar(scalar, VX_SCALAR_ATTRIBUTE_TYPE, &type, sizeof(type)));
STATUS_ERROR_CHECK(vxReadScalarValue(scalar, &value));
if ((value != vx_true_e && value != vx_false_e) || type != VX_TYPE_BOOL)
status = VX_ERROR_INVALID_VALUE;
vxReleaseScalar(&scalar);
}
else if (index == 8)
{
vx_scalar scalar = 0; vx_enum type = 0; vx_bool value = vx_true_e;
STATUS_ERROR_CHECK(vxQueryParameter(param, VX_PARAMETER_ATTRIBUTE_REF, &scalar, sizeof(scalar)));
STATUS_ERROR_CHECK(vxQueryScalar(scalar, VX_SCALAR_ATTRIBUTE_TYPE, &type, sizeof(type)));
STATUS_ERROR_CHECK(vxReadScalarValue(scalar, &value));
if ((value != vx_true_e && value != vx_false_e) || type != VX_TYPE_BOOL)
status = VX_ERROR_INVALID_VALUE;
vxReleaseScalar(&scalar);
}
vxReleaseParameter(¶m);
return status;
}
