void userCalculatePerspectiveTransformFromLK(vx_matrix matrix_forward, vx_matrix matrix_backward, vx_array old_features,
vx_array new_features)
{
vx_float32 mat1[3][3];
vx_float32 mat2[3][3];
vx_size olen = 0;
vx_size nlen = 0;
vx_keypoint_t *old_features_ptr = NULL;
vx_keypoint_t *new_features_ptr = NULL;
vx_size old_features_stride = 0;
vx_size new_features_stride = 0;
vx_uint32 ind;
//vx_float32 *A = malloc(olen+nlen);
//vx_float32 *b = malloc(olen+nlen);
vxQueryArray(old_features, VX_ARRAY_ATTRIBUTE_NUMITEMS, &olen, sizeof(olen));
vxQueryArray(new_features, VX_ARRAY_ATTRIBUTE_NUMITEMS, &nlen, sizeof(nlen));
if (olen != nlen)
return;
vxAccessArrayRange(old_features, 0, olen, &old_features_stride, (void **) &old_features_ptr, VX_READ_ONLY);
vxAccessArrayRange(new_features, 0, nlen, &new_features_stride, (void **) &new_features_ptr, VX_READ_ONLY);
/*! \internal do least square algorithm that find perspective transform */
/*! \see "Computer Vision Algorithm and Application by Richard Szeliski section 6.1.3 */
for (ind = 0; ind < nlen; ind++)
{
//vx_int32 matrix_index = 2 * ind;
//vx_keypoint_t old_kp = vxArrayItem(vx_keypoint_t, old_features_ptr, ind, old_features_stride);
//vx_keypoint_t new_kp = vxArrayItem(vx_keypoint_t, new_features_ptr, ind, new_features_stride);
/*
A[matrix_index] = {old_kp.x, old_kp.y, 1,
0, 0, 0,
-old_kp.x*new_kp.x,
-old_kp.x*new_kp.y};
A[matrix_index+1] = {0, 0, 0,
old_kp.x,
old_kp.y,
1,
-old_kp.y*new_kp.x,
-old_kp.y*new_kp.y};
b[matrix_index] = new_kp.x - old_kp.x;
b[matrix_index+1] = new_kp.y - old_kp.y;
*/
}
//least_square_divide(A,b, mat1);
//inverse_matrix(mat1, mat2);
vxCommitArrayRange(old_features, 0, 0, old_features_ptr);
vxCommitArrayRange(new_features, 0, 0, new_features_ptr);
vxWriteMatrix(matrix_forward, mat1);
vxWriteMatrix(matrix_backward, mat2);
}
int CVxParamMatrix::ReadFrame(int frameNumber)
{
// check if there is no user request to read
if (m_fileNameRead.length() < 1) return 0;
// make sure buffer has been allocated
if (!m_bufForAccess) NULLPTR_CHECK(m_bufForAccess = new vx_uint8[m_size]);
// for single frame reads, there is no need to read it 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 == m_captureFrameStart) {
ReportError("ERROR: Unable to open: %s\n", fileName);
}
else {
return 1; // end of sequence detected for multiframe sequences
}
}
int status = 0;
if (m_readFileIsBinary) {
if (fread(m_bufForAccess, 1, m_size, fp) != m_size)
status = -1;
}
else {
for (vx_size index = 0; index < (m_columns * m_rows); index++) {
if (m_data_type == VX_TYPE_INT32 || m_data_type == VX_TYPE_UINT8) {
vx_uint32 value;
if (fscanf(fp, "%i", &value) != 1) {
status = -1;
break;
}
if (m_data_type == VX_TYPE_UINT8) ((vx_uint8 *)m_bufForAccess)[index] = (vx_uint8)value;
else ((vx_int32 *)m_bufForAccess)[index] = value;
}
else if (m_data_type == VX_TYPE_FLOAT32) {
if (fscanf(fp, "%g", &((vx_float32 *)m_bufForAccess)[index]) != 1) {
status = -1;
break;
}
}
else ReportError("ERROR: matrix ascii read option not support for data_type of %s\n", GetVxObjectName());
}
}
ERROR_CHECK(vxWriteMatrix(m_matrix, m_bufForAccess));
fclose(fp);
if (status < 0)
ReportError("ERROR: detected EOF on matrix input file: %s\n", fileName);
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;
}
int CVxParamMatrix::InitializeIO(vx_context context, vx_graph graph, vx_reference ref, const char * io_params)
{
// save reference object and get object attributes
m_vxObjRef = ref;
m_matrix = (vx_matrix)m_vxObjRef;
ERROR_CHECK(vxQueryMatrix(m_matrix, VX_MATRIX_ATTRIBUTE_TYPE, &m_data_type, sizeof(m_data_type)));
ERROR_CHECK(vxQueryMatrix(m_matrix, VX_MATRIX_ATTRIBUTE_COLUMNS, &m_columns, sizeof(m_columns)));
ERROR_CHECK(vxQueryMatrix(m_matrix, VX_MATRIX_ATTRIBUTE_ROWS, &m_rows, sizeof(m_rows)));
ERROR_CHECK(vxQueryMatrix(m_matrix, VX_MATRIX_ATTRIBUTE_SIZE, &m_size, sizeof(m_size)));
// process I/O parameters
if (*io_params == ':') io_params++;
while (*io_params) {
char ioType[64], fileName[256];
io_params = ScanParameters(io_params, "<io-operation>,<parameter>", "s,S", ioType, fileName);
if (!_stricmp(ioType, "read"))
{ // read request syntax: read,<fileName>[,ascii|binary]
m_fileNameRead.assign(RootDirUpdated(fileName));
m_fileNameForReadHasIndex = (m_fileNameRead.find("%") != m_fileNameRead.npos) ? true : false;
m_readFileIsBinary = (m_fileNameRead.find(".txt") != m_fileNameRead.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary", ",s", option);
if (!_stricmp(option, "ascii")) {
m_readFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_readFileIsBinary = true;
}
else ReportError("ERROR: invalid matrix read option: %s\n", option);
}
}
else if (!_stricmp(ioType, "write"))
{ // write request syntax: write,<fileName>[,ascii|binary]
m_fileNameWrite.assign(RootDirUpdated(fileName));
m_writeFileIsBinary = (m_fileNameWrite.find(".txt") != m_fileNameWrite.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary", ",s", option);
if (!_stricmp(option, "ascii")) {
m_writeFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_writeFileIsBinary = true;
}
else ReportError("ERROR: invalid matrix write option: %s\n", option);
}
}
else if (!_stricmp(ioType, "compare"))
{ // compare request syntax: compare,<fileName>[,ascii|binary][,err{<tolerance>}]
m_fileNameCompare.assign(RootDirUpdated(fileName));
m_compareFileIsBinary = (m_fileNameCompare.find(".txt") != m_fileNameCompare.npos) ? false : true;
while (*io_params == ',') {
char option[64];
io_params = ScanParameters(io_params, ",ascii|binary|err{<tolerance>}", ",s", option);
if (!_stricmp(option, "ascii")) {
m_compareFileIsBinary = false;
}
else if (!_stricmp(option, "binary")) {
m_compareFileIsBinary = true;
}
else if (!_strnicmp(option, "err{", 4)) {
ScanParameters(&option[3], "{<tolerance>}", "{f}", &m_errTolerance);
}
else ReportError("ERROR: invalid matrix compare option: %s\n", option);
}
}
else if (!_stricmp(ioType, "init"))
{ // write request syntax: init,{<value1>;<value2>;...<valueN>}
NULLPTR_CHECK(m_bufForAccess = new vx_uint8[m_size]);
vx_size index = 0; char fmt[3] = { '{', (m_data_type == VX_TYPE_FLOAT32) ? 'f' : 'd', 0 };
for (const char * s = fileName; *s && index < (m_columns * m_rows); fmt[0] = ';', index++) {
if (m_data_type == VX_TYPE_INT32 || m_data_type == VX_TYPE_UINT8) {
vx_uint32 value;
s = ScanParameters(s, "<value>", fmt, &value);
if (m_data_type == VX_TYPE_UINT8) ((vx_uint8 *)m_bufForAccess)[index] = (vx_uint8)value;
else ((vx_int32 *)m_bufForAccess)[index] = value;
}
else if (m_data_type == VX_TYPE_FLOAT32) {
s = ScanParameters(s, "<value>", fmt, &((vx_float32 *)m_bufForAccess)[index]);
}
else ReportError("ERROR: matrix init option not support for data_type of %s\n", GetVxObjectName());
}
if (index < (m_columns * m_rows)) ReportError("ERROR: matrix init have too few values: %s\n", fileName);
ERROR_CHECK(vxWriteMatrix(m_matrix, m_bufForAccess));
}
else if (!_stricmp(ioType, "directive") && !_stricmp(fileName, "readonly")) {
ERROR_CHECK(vxDirective((vx_reference)m_matrix, VX_DIRECTIVE_AMD_READ_ONLY));
}
else if (!_stricmp(ioType, "ui") && !_strnicmp(fileName, "f", 1) && m_data_type == VX_TYPE_FLOAT32 && m_columns == 3 && m_rows == 3) {
int id = 0;
float valueR = 200.0f, valueInc = 0.5f;
if (sscanf(&fileName[1], "%d,%g,%g", &id, &valueR, &valueInc) != 3) {
printf("ERROR: invalid matrix UI configuration '%s'\n", fileName);
return -1;
}
id--;
GuiTrackBarInitializeMatrix((vx_reference)m_matrix, id, valueR, valueInc);
GuiTrackBarProcessKey(0); // just initialize the matrix
}
else ReportError("ERROR: invalid matrix operation: %s\n", ioType);
if (*io_params == ':') io_params++;
else if (*io_params) ReportError("ERROR: unexpected character sequence in parameter specification: %s\n", io_params);
}
return 0;
}
