vx_status vx_example_warp_affine(vx_context context, vx_image src, vx_image dst)
{
vx_float32 a = 1.0f, b = 0.0f, c = 0.0f, d = 1.0f, e = 0.0f , f = 0.0f;
//! [warp affine]
// x0 = a x + b y + c;
// y0 = d x + e y + f;
vx_float32 mat[3][2] = {
{a, d}, // 'x' coefficients
{b, e}, // 'y' coefficients
{c, f}, // 'offsets'
};
vx_matrix matrix = vxCreateMatrix(context, VX_TYPE_FLOAT32, 2, 3);
vxAccessMatrix(matrix, NULL);
vxCommitMatrix(matrix, mat);
//! [warp affine]
return vxuWarpAffine(context, src, matrix, VX_INTERPOLATION_TYPE_NEAREST_NEIGHBOR, dst);
}
vx_status vx_example_warp_perspective(vx_context context, vx_image src, vx_image dst)
{
vx_float32 a = 1.0f, b = 0.0f, c = 0.0f, d = 1.0f, e = 0.0f, f = 0.0f, g = 1.0f, h = 0.0f, i = 0.0f;
//! [warp perspective]
// x0 = a x + b y + c;
// y0 = d x + e y + f;
// z0 = g x + h y + i;
vx_float32 mat[3][3] = {
{a, d, g}, // 'x' coefficients
{b, e, h}, // 'y' coefficients
{c, f, i}, // 'offsets'
};
vx_matrix matrix = vxCreateMatrix(context, VX_TYPE_FLOAT32, 3, 3);
vxAccessMatrix(matrix, NULL);
vxCommitMatrix(matrix, mat);
//! [warp perspective]
return vxuWarpPerspective(context, src, matrix, VX_INTERPOLATION_TYPE_NEAREST_NEIGHBOR, dst);
}
int CVxParamMatrix::Initialize(vx_context context, vx_graph graph, const char * desc)
{
// get object parameters and create object
char objType[64], data_type[64];
const char * ioParams = ScanParameters(desc, "matrix:<data-type>,<columns>,<rows>", "s:s,D,D", objType, data_type, &m_columns, &m_rows);
if (!_stricmp(objType, "matrix")) {
m_data_type = ovxName2Enum(data_type);
m_matrix = vxCreateMatrix(context, m_data_type, m_columns, m_rows);
}
else ReportError("ERROR: unsupported matrix type: %s\n", desc);
vx_status ovxStatus = vxGetStatus((vx_reference)m_matrix);
if (ovxStatus != VX_SUCCESS){
printf("ERROR: matrix creation failed => %d (%s)\n", ovxStatus, ovxEnum2Name(ovxStatus));
if (m_matrix) vxReleaseMatrix(&m_matrix);
throw - 1;
}
m_vxObjRef = (vx_reference)m_matrix;
// io initialize
return InitializeIO(context, graph, m_vxObjRef, ioParams);
}
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;
}
/*!
* \brief An example of an super resolution algorithm.
* \ingroup group_example
*/
int example_super_resolution(int argc, char *argv[])
{
vx_status status = VX_SUCCESS;
vx_uint32 image_index = 0, max_num_images = 4;
vx_uint32 width = 640;
vx_uint32 i = 0;
vx_uint32 winSize = 32;
vx_uint32 height = 480;
vx_int32 sens_thresh = 20;
vx_float32 alpha = 0.2f;
vx_float32 tau = 0.5f;
vx_enum criteria = VX_TERM_CRITERIA_BOTH; // lk params
vx_float32 epsilon = 0.01;
vx_int32 num_iterations = 10;
vx_bool use_initial_estimate = vx_true_e;
vx_int32 min_distance = 5; // harris params
vx_float32 sensitivity = 0.04;
vx_int32 gradient_size = 3;
vx_int32 block_size = 3;
vx_context context = vxCreateContext();
vx_scalar alpha_s = vxCreateScalar(context, VX_TYPE_FLOAT32, &alpha);
vx_scalar tau_s = vxCreateScalar(context, VX_TYPE_FLOAT32, &tau);
vx_matrix matrix_forward = vxCreateMatrix(context, VX_TYPE_FLOAT32, 3, 3);
vx_matrix matrix_backwords = vxCreateMatrix(context, VX_TYPE_FLOAT32, 3, 3);
vx_array old_features = vxCreateArray(context, VX_TYPE_KEYPOINT, 1000);
vx_array new_features = vxCreateArray(context, VX_TYPE_KEYPOINT, 1000);
vx_scalar epsilon_s = vxCreateScalar(context, VX_TYPE_FLOAT32, &epsilon);
vx_scalar num_iterations_s = vxCreateScalar(context, VX_TYPE_INT32, &num_iterations);
vx_scalar use_initial_estimate_s = vxCreateScalar(context, VX_TYPE_BOOL, &use_initial_estimate);
vx_scalar min_distance_s = vxCreateScalar(context, VX_TYPE_INT32, &min_distance);
vx_scalar sensitivity_s = vxCreateScalar(context, VX_TYPE_FLOAT32, &sensitivity);
vx_scalar sens_thresh_s = vxCreateScalar(context, VX_TYPE_INT32, &sens_thresh);
vx_scalar num_corners = vxCreateScalar(context, VX_TYPE_SIZE, NULL);
if (vxGetStatus((vx_reference)context) == VX_SUCCESS)
{
vx_image images[] =
{ vxCreateImage(context, width, height, VX_DF_IMAGE_UYVY), // index 0:
vxCreateImage(context, width, height, VX_DF_IMAGE_U8), // index 1: Get Y channel
vxCreateImage(context, width * 2, height * 2, VX_DF_IMAGE_U8), // index 2: scale up to high res.
vxCreateImage(context, width * 2, height * 2, VX_DF_IMAGE_U8), // index 3: back wrap: transform to the original Image.
vxCreateImage(context, width * 2, height * 2, VX_DF_IMAGE_U8), // index 4: guassian blur
vxCreateImage(context, width, height, VX_DF_IMAGE_U8), // index 5: scale down
vxCreateImage(context, width, height, VX_DF_IMAGE_S16), // index 6: Subtract the transformed Image with original moved Image
vxCreateImage(context, width * 2, height * 2, VX_DF_IMAGE_S16), // index 7: Scale Up the delta image.
vxCreateImage(context, width * 2, height * 2, VX_DF_IMAGE_S16), // index 8: Guassian blur the delta Image
vxCreateImage(context, width * 2, height * 2, VX_DF_IMAGE_S16), // index 9: forward wrap: tranform the deltas back to the high res Image
vxCreateImage(context, width * 2, height * 2, VX_DF_IMAGE_U8), // index 10: accumulate sum?
vxCreateImage(context, width, height, VX_DF_IMAGE_U8), // index 11: Get U channel
vxCreateImage(context, width * 2, height * 2, VX_DF_IMAGE_U8), // index 12: scale up to high res.
vxCreateImage(context, width, height, VX_DF_IMAGE_U8), // index 13: Get V channel
vxCreateImage(context, width * 2, height * 2, VX_DF_IMAGE_U8), // index 14: scale up to high res.
vxCreateImage(context, width, height, VX_DF_IMAGE_UYVY), // index 15: output image
vxCreateImage(context, width * 2, height * 2, VX_DF_IMAGE_U8), // index 16: original y image scaled
vxCreateImage(context, width * 2, height * 2, VX_DF_IMAGE_U8), // index 17: difference image for last calculation
};
vx_pyramid pyramid_new = vxCreatePyramid(context, 4, 2, width, height, VX_DF_IMAGE_U8);
vx_pyramid pyramid_old = vxCreatePyramid(context, 4, 2, width, height, VX_DF_IMAGE_U8);
vx_graph graphs[] =
{ vxCreateGraph(context), vxCreateGraph(context), vxCreateGraph(context), vxCreateGraph(context), };
vxLoadKernels(context, "openvx-debug");
if (vxGetStatus((vx_reference)graphs[0]) == VX_SUCCESS)
{
vxChannelExtractNode(graphs[0], images[0], VX_CHANNEL_Y, images[1]); // One iteration of super resolution calculation
vxScaleImageNode(graphs[0], images[1], images[2], VX_INTERPOLATION_TYPE_BILINEAR);
vxWarpPerspectiveNode(graphs[0], images[2], matrix_forward, 0, images[3]);
vxGaussian3x3Node(graphs[0], images[3], images[4]);
vxScaleImageNode(graphs[0], images[4], images[5], VX_INTERPOLATION_TYPE_BILINEAR);
vxSubtractNode(graphs[0], images[5], images[16], VX_CONVERT_POLICY_SATURATE, images[6]);
vxScaleImageNode(graphs[0], images[6], images[7], VX_INTERPOLATION_TYPE_BILINEAR);
vxGaussian3x3Node(graphs[0], images[7], images[8]);
vxWarpPerspectiveNode(graphs[0], images[8], matrix_backwords, 0, images[9]);
vxAccumulateWeightedImageNode(graphs[0], images[9], alpha_s, images[10]);
}
if (vxGetStatus((vx_reference)graphs[1]) == VX_SUCCESS)
{
vxChannelExtractNode(graphs[1], images[0], VX_CHANNEL_Y, images[1]); // One iteration of super resolution calculation
vxGaussianPyramidNode(graphs[1], images[1], pyramid_new);
vxOpticalFlowPyrLKNode(graphs[1], pyramid_old, pyramid_new, old_features, old_features, new_features,
criteria, epsilon_s, num_iterations_s, use_initial_estimate_s, winSize);
}
if (vxGetStatus((vx_reference)graphs[2]) == VX_SUCCESS)
{
vxChannelExtractNode(graphs[2], images[0], VX_CHANNEL_Y, images[1]); // One iteration of super resolution calculation
vxHarrisCornersNode(graphs[2], images[1], sens_thresh_s, min_distance_s, sensitivity_s, gradient_size,
block_size, old_features, num_corners);
vxGaussianPyramidNode(graphs[2], images[1], pyramid_old);
vxScaleImageNode(graphs[2], images[1], images[16], VX_INTERPOLATION_TYPE_BILINEAR);
}
if (vxGetStatus((vx_reference)graphs[3]) == VX_SUCCESS)
{
vxSubtractNode(graphs[3], images[10], images[16], VX_CONVERT_POLICY_SATURATE, images[17]);
vxAccumulateWeightedImageNode(graphs[3], images[17], tau_s, images[16]);
vxChannelExtractNode(graphs[3], images[16], VX_CHANNEL_U, images[11]);
vxScaleImageNode(graphs[3], images[11], images[12], VX_INTERPOLATION_TYPE_BILINEAR); // upscale the u channel
vxChannelExtractNode(graphs[3], images[0], VX_CHANNEL_V, images[13]);
vxScaleImageNode(graphs[3], images[13], images[14], VX_INTERPOLATION_TYPE_BILINEAR); // upscale the v channel
vxChannelCombineNode(graphs[3], images[10], images[12], images[14], 0, images[15]); // recombine the channels
}
status = VX_SUCCESS;
status |= vxVerifyGraph(graphs[0]);
status |= vxVerifyGraph(graphs[1]);
status |= vxVerifyGraph(graphs[2]);
status |= vxVerifyGraph(graphs[3]);
if (status == VX_SUCCESS)
{
/* read the initial image in */
status |= vxuFReadImage(context, "c:\\work\\super_res\\superres_1_UYVY.yuv", images[0]);
/* compute the "old" pyramid */
status |= vxProcessGraph(graphs[2]);
/* for each input image, read it in and run graphs[1] and [0]. */
for (image_index = 1; image_index < max_num_images; image_index++)
{
char filename[256];
sprintf(filename, "c:\\work\\super_res\\superres_%d_UYVY.yuv", image_index + 1);
status |= vxuFReadImage(context, filename, images[0]);
status |= vxProcessGraph(graphs[1]);
userCalculatePerspectiveTransformFromLK(matrix_forward, matrix_backwords, old_features, new_features);
status |= vxProcessGraph(graphs[0]);
}
/* run the final graph */
status |= vxProcessGraph(graphs[3]);
/* save the output */
status |= vxuFWriteImage(context, images[15], "superres_UYVY.yuv");
}
vxReleaseGraph(&graphs[0]);
vxReleaseGraph(&graphs[1]);
vxReleaseGraph(&graphs[2]);
vxReleaseGraph(&graphs[3]);
for (i = 0; i < dimof(images); i++)
{
vxReleaseImage(&images[i]);
}
vxReleasePyramid(&pyramid_new);
vxReleasePyramid(&pyramid_old);
}
vxReleaseMatrix(&matrix_forward);
vxReleaseMatrix(&matrix_backwords);
vxReleaseScalar(&alpha_s);
vxReleaseScalar(&tau_s);
/* Release the context last */
vxReleaseContext(&context);
return status;
}