Sample Code - MISB 0903 Converter Demo Application (C++)

// STD0903DemoApp.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <stdio.h>
#include <memory.h>
#include <comutil.h>
#include <iostream>
#include <fstream>
#include "ISTD0903Conv.h"
#include "Windows.h"
#include <Oleauto.h>
#include <OaIdl.h>
#include <iostream>
#include <iomanip>


#ifdef _DEBUG
# pragma comment(lib, "comsuppwd.lib")
#else
# pragma comment(lib, "comsuppw.lib")
#endif
# pragma comment(lib, "wbemuuid.lib")

#define TARGET_PACKS 1
#define NUMBER_OF_POINTS 10

#pragma warning(push)
#pragma warning(disable : 4482)

using namespace std;

bool exists(const char* path);
void ReadVmtiFileAndShowContent(ISTD0903Converter* pVmtiConv, const char* file);
unsigned char* FillAndEncodeVMTIBuffer(ISTD0903Converter* pVmtiConv, VMTI_Packet* pPckt, size_t& length);
VMTI_Packet* DecodeVMTIBuffer(ISTD0903Converter* pVmtiConv, const unsigned char* pBuf, const size_t length);
VTrackerLDS* CreateVTrackerLDS(unsigned __int64 id);

unsigned char* ReadBinData(const char* path, size_t& l);
void PrintVmtiPacket(ISTD0903Converter* conv, VMTI_Packet* pEncPckt);
void PrintData(VARIANT& vt);
void PrintHierarchyData(ISTD0903Converter* conv, VTargetPackTag tag, VARIANT& vt);


int main(int argc, char* argv[])
{
        size_t length;
        unsigned char* pBuf;

        // NOTE, Demo version will add a 1 sec delay and will process a limited number of packets only!!!

        // Create VMTI Converter
        ISTD0903Converter* pVmtiConv = CreateSTD0903Converter();
        pVmtiConv->ConfigureEncoder(74, false);

        // Sample 1. Read from file, doecode and show the content
        if (argc < 2)
        {
                cout << "No bin file argument provided";
        }
        else
        {
                string path = argv[1]; //"\\VmtiTest.bin";      
                if (exists(path.c_str()))
                        ReadVmtiFileAndShowContent(pVmtiConv, path.c_str());
                else
                        cout << "File " << path << " not found";
        }

        // Sample 2. Create the VMTI_Packet 
        VMTI_Packet* pEncPckt = new  VMTI_Packet();

        // Encode VMTI buffer   
        pBuf = FillAndEncodeVMTIBuffer(pVmtiConv, pEncPckt, length);

        // Examine the content  
        PrintVmtiPacket(pVmtiConv, pEncPckt);
        delete pEncPckt;


        // Sample 3. Decode it back
        cout << "Decode Packet";
        VMTI_Packet* pDecPckt = DecodeVMTIBuffer(pVmtiConv, pBuf, length);

        // Examine the content  
        cout << "Examine the content";
        PrintVmtiPacket(pVmtiConv, pDecPckt);

        // Delete decoded packet
        delete pDecPckt;        

        // Delete converter
        delete pVmtiConv;

        return 0;
}


void ReadVmtiFileAndShowContent(ISTD0903Converter* pVmtiConv, const char* filePath)
{
        size_t l;
        // Read sample buffer
        unsigned char* sampleBuf = ReadBinData(filePath, l);

        // Create the VMTI packet to hold the decoded data 
        VMTI_Packet* pPcktDec = new VMTI_Packet();

        // Decode the buffer
        pVmtiConv->Decode(sampleBuf, l, pPcktDec);

        // If you only need to decode a payload (your buffer does not contain key and length fields, use the DecodePayload method.
        //pVmtiConv->DecodePayload(sampleBuf + 3, l - 3, pPcktDec);

        // Or, if a buffer contains a payload only 
        //pVmtiConv->DecodePayload(sampleBuf, l, pPcktDec);

        // Examine the content  
        PrintVmtiPacket(pVmtiConv, pPcktDec);

        // Delete Packet
        delete pPcktDec;

        // Delete sampleBuf
        delete sampleBuf;
}

// Create and encode VMTI packet
unsigned char* FillAndEncodeVMTIBuffer(ISTD0903Converter* pVmtiConv, VMTI_Packet* pPckt, size_t& length)
{
        //unsigned __int64 uval_64 = 1231798102000000;  // Convert UNIX Time Stamp (Tag 2) to 8 bytes (unsigned int64) LDS
        //pPckt->SetUnixTimeStamp(uval_64);
        // or just set a current time

        pPckt->SetUnixTimeStampNow();
        pPckt->SetVmtiSystemName("Test System");
        pPckt->SetVmtiLDSVersion(3);
        pPckt->SetTotalNumberOfTargets(2);
        pPckt->SetNumberOfReportedTargets(2);
        pPckt->SetVideoFrameNumber(1);
        pPckt->SetFrameWidth(720);
        pPckt->SetFrameHeight(480);
        pPckt->SetVmtiSourceSensor("EO Nose");
        pPckt->SetVmtiSensorHorizontalFOV(12.5);
        pPckt->SetVmtiSensorVerticalFOV(12.5);

        // Create TARGET_PACKS target packs
        for (int i = 0; i < TARGET_PACKS; i++)
        {
                // Create a target pack and set the values
                VTargetPack* pvPack1 = new VTargetPack(27 + i, 3);
                
                pvPack1->SetTargetCentroidPixel(409600);                                // Tag 1
                pvPack1->SetBoundingBoxTopLeftPixel(409600);                    // Tag 2
                pvPack1->SetBoundingBoxBottomRightPixel(409600);                // Tag 3
                pvPack1->SetTargetPriority(1);                                                  // Tag 4
                pvPack1->SetTargetConfidenceLevel(80);                                  // Tag 5
                pvPack1->SetNewDetectionFlag((unsigned short)4);                // Tag 6
                pvPack1->SetPercentageOfTargetPixels(50);                               // Tag 7
                pvPack1->SetTargetColor(0x558833);                                              // Tag 8
                pvPack1->SetTargetIntensity(13140);                                             // Tag 9
                pvPack1->SetTargetLocationLatOffset(10.0);                              // Tag 10
                pvPack1->SetTargetLocationLonOffset(10.0);                              // Tag 11
                pvPack1->SetTargetHeight(1000);                                                 // Tag 12
                pvPack1->SetBoundingBoxTopLeftLatOffset(-0.5);                  // Tag 13
                pvPack1->SetBoundingBoxTopLeftLonOffset(0.5);                   // Tag 14
                pvPack1->SetBoundingBoxBottomRightLatOffset(-10.0);             // Tag 15
                pvPack1->SetBoundingBoxBottomRightLonOffset(10.0);              // Tag 16
                
                LocationType tLoc = LocationType(43, 110, 10000.0);
                pvPack1->SetTargetLocation(tLoc);                                               // Tag 17
        
                // Create VTrackerLDS, set the values and add to the pack
                VTrackerLDS* vTLds = CreateVTrackerLDS(10);
                pvPack1->SetVTrackerLDS(vTLds);
                delete vTLds;           // delete it, we no longer need it.

                // Add to the packet
                pPckt->AddVTargetPack(pvPack1);
        }

        // Encode and return the buffer
        return pVmtiConv->Encode(pPckt, length);
}


VMTI_Packet* DecodeVMTIBuffer(ISTD0903Converter* pVmtiConv, const unsigned char* pBuf, const size_t length)
{
        VMTI_Packet* pPckt = new  VMTI_Packet();

        pVmtiConv->Decode(pBuf, length, pPckt);

        return pPckt;
}

VTrackerLDS* CreateVTrackerLDS(unsigned __int64 id)
{
        VTrackerLDS* vLds = new VTrackerLDS(id);

        vLds->SetDetectionStatus(TrackStatus::Active);
        vLds->SetStartTimeStamp(VTrackerLDS::GetTimeStampNow());
        vLds->SetEndTimeStamp(VTrackerLDS::GetTimeStampNow());
        vLds->SetAlgorithm("Algo1");
        vLds->SetConfidence(50);
        vLds->SetNumberOfPoints(NUMBER_OF_POINTS);

        // Add Locus (let's set NUMBER_OF_POINTS items)
        for (int i = 0; i < NUMBER_OF_POINTS; i++)
        {
                vLds->AddToLocus(LocationType(34.5 + (double)i / 1000.0, 32.0 + (double)i / 1000.0, 10.0));
        }

        vLds->SetVelocity(VAType(300.0, 200.0, 100.0));
        vLds->SetAcceleration(VAType(0.2, 0.4, 0.6));

        return vLds;
}


void PrintVmtiPacket(ISTD0903Converter* pVmtiConv, VMTI_Packet* pPcktDec)
{
        VARIANT vt;

        // Iterate existing local data items
        TargetLocalDataIterator ld_it;
        for (ld_it = pPcktDec->GetVTargetLocalData().begin(); ld_it != pPcktDec->GetVTargetLocalData().end(); ld_it++)
        {
                VariantInit(&vt);
                VTargetLocalDataTag tag = (*ld_it).first;
                pPcktDec->DecodeValue(tag, &vt);
                cout << "Tag " << tag << " : " << pVmtiConv->TargetLocalDataTagToString(tag) << " - ";
                PrintData(vt);
                if (tag == VTargetLocalDataTag::UNIX_Time_Stamp)
                        cout << " (" << pVmtiConv->TargetLocalDataToHRString(tag, vt) << ")";
                cout << endl;
                VariantClear(&vt);
        }

        // Iterate existing target series items
        TargetSeriesIterator ts_it;
        for (ts_it = pPcktDec->GetVTargetSeries().begin(); ts_it != pPcktDec->GetVTargetSeries().end(); ts_it++)
        {
                VTargetPack* pTargetPack = *ts_it;
                pTargetPack->SetRevision(pPcktDec->Revision);
                TargetPackIterator tp_it;

                cout << "\tTarget Id - " << pTargetPack->TargetID << endl;
                for (tp_it = pTargetPack->GetVTargetPackItems().begin(); tp_it != pTargetPack->GetVTargetPackItems().end(); tp_it++)
                {
                        VariantInit(&vt);
                        VTargetPackTag tag = (*tp_it).first;
                        pTargetPack->DecodeValue(tag, &vt);
                        cout << "\t\tTag " << tag << " : " << pVmtiConv->TargetPackTagToString(tag) << " - ";
                        if (tag < Target_Location)
                                PrintData(vt);
                        else
                        {
                                PrintHierarchyData(pVmtiConv, tag, vt);
                        }
                        cout << endl;
                        VariantClear(&vt);
                }
        }
        cout << endl;
}


// Prinf out the data value
void PrintData(VARIANT& vt)
{
        switch (vt.vt)
        {
        case VT_BSTR:
                cout << (char*)(_bstr_t)vt;
                break;

        case VT_UI1:
        case VT_UI2:
        case VT_UI4:
                cout << vt.uintVal;
                break;
        case VT_UI8:
                cout << vt.ullVal;
                break;
        case VT_R4:
                cout << vt.fltVal;
                break;

        case VT_R8:
                cout << vt.dblVal;
                break;
        }
}

void PrintHierarchyData(ISTD0903Converter* conv, VTargetPackTag tag, VARIANT& vt)
{
        switch (tag)
        {
                case Target_Location:
                {
                        SafeArrayLock(vt.parray);
                        void* pArrayData = NULL;
                        SafeArrayAccessData(vt.parray, &pArrayData);
                        long uBound;
                        HRESULT hresult;
                        hresult = SafeArrayGetUBound(vt.parray, 1, &uBound);
                        if (!FAILED(hresult))
                        {                               
                                LocationType locus((unsigned char*)pArrayData, (size_t)uBound + 1);
                        
                                if (locus.GetMode() == LocationMode::GeospatialOnly)
                                        cout << "\tLat: " << locus.Latitude << "  Lon: " << locus.Longitude << "  Alt: " << std::fixed << std::setprecision(2) << locus.Height << std::setprecision(4);
                                else
                                        if (locus.GetMode() == LocationMode::GeospatialAndDeviations)
                                                cout << " Sigma Lat: " << locus.SigmaLatitude << " Sigma Lon: " << locus.SigmaLongitude << " Sigma Alt: " << locus.SigmaHeight;
                                        else
                                                cout << " RhoLatLon: " << locus.RhoLatLon << " RhoLatHt: " << locus.RhoLatHt << " RhoLonH: " << locus.RhoLonHt;
                                cout << endl;
                                SafeArrayUnaccessData(vt.parray);
                        }
                }
                break;

        case VObject:

                break;
        case VFeature_LDS:

                break;
        case VTracker_LDS:
        {
                SafeArrayLock(vt.parray);
                void* pArrayData = NULL;
                SafeArrayAccessData(vt.parray, &pArrayData);
                long uBound;
                HRESULT hresult;
                hresult = SafeArrayGetUBound(vt.parray, 1, &uBound);
                if (!FAILED(hresult))
                {
                        VTrackerLDS vtLds((char*)pArrayData, uBound);
                        cout << "(TargetId - " << vtLds.TargetID << ")" << endl;
                        map<VTrackerLDSTag, VTargetItem*>::iterator it;
                        for (it = vtLds.m_LDSItems.begin(); it != vtLds.m_LDSItems.end(); it++)
                        {
                                VTrackerLDSTag tag = (*it).first;
                                if (tag != VTrackerLDSTag::Locus && tag != VTrackerLDSTag::Velocity && tag != VTrackerLDSTag::Acceleration)
                                {
                                        cout << "\t\t\tTag " << tag << " : " << conv->TrackerLocalDataTagToString(tag) << " - " << conv->TrackerLocalDataItemToHRString(tag, *(*it).second);
                                        cout << endl;
                                }
                        }

                        if (!vtLds.m_Locus.empty())
                        {
                                vector<LocationType>::iterator itLocus;
                                cout << "\t\t\t\tLocus:" << endl;
                                cout << std::fixed << std::setprecision(4);
                                for (itLocus = vtLds.m_Locus.begin(); itLocus != vtLds.m_Locus.end(); itLocus++)
                                {
                                        if (itLocus->GetMode() == LocationMode::GeospatialOnly)
                                                cout << "\t\t\t\tLat: " << itLocus->Latitude << "  Lon: " << itLocus->Longitude << "  Alt: " << std::fixed << std::setprecision(2) << itLocus->Height << std::setprecision(4);
                                        else
                                                if (itLocus->GetMode() == LocationMode::GeospatialAndDeviations)
                                                        cout << " Sigma Lat: " << itLocus->SigmaLatitude << " Sigma Lon: " << itLocus->SigmaLongitude << " Sigma Alt: " << itLocus->SigmaHeight;
                                                else
                                                        cout << " RhoLatLon: " << itLocus->RhoLatLon << " RhoLatHt: " << itLocus->RhoLatHt << " RhoLonH: " << itLocus->RhoLonHt;
                                        cout << endl;
                                }
                        }

                        if (vtLds.m_pVelocity){
                                cout << "\t\t\t\tVelocity:" << endl;
                                cout << std::fixed << std::setprecision(4);
                                if (vtLds.m_pVelocity->GetMode() == VAMode::ComponentOnly)
                                        cout << "\t\t\t\tX: " << vtLds.m_pVelocity->X_Component << "  Y: " << vtLds.m_pVelocity->Y_Component << "  Z: " << vtLds.m_pVelocity->Z_Component;
                                else
                                        if (vtLds.m_pVelocity->GetMode() == VAMode::ComponentAndDeviations)
                                                cout << " Sigma X: " << vtLds.m_pVelocity->SigmaX << " Sigma Y: " << vtLds.m_pVelocity->SigmaY << " Sigma Z: " << vtLds.m_pVelocity->SigmaZ;
                                        else
                                                cout << " Rho_X_Y: " << vtLds.m_pVelocity->Rho_X_Y << " Rho_X_Z: " << vtLds.m_pVelocity->Rho_X_Z << " Rho_Y_Z: " << vtLds.m_pVelocity->Rho_Y_Z;
                                cout << endl;
                        }

                        if (vtLds.m_pAcceleration){
                                cout << "\t\t\t\tAcceleration:" << endl;
                                cout << std::fixed << std::setprecision(4);
                                if (vtLds.m_pAcceleration->GetMode() == VAMode::ComponentOnly)
                                        cout << "\t\t\t\tX: " << vtLds.m_pAcceleration->X_Component << "  Y: " << vtLds.m_pAcceleration->Y_Component << "  Z: " << vtLds.m_pAcceleration->Z_Component;
                                else
                                        if (vtLds.m_pAcceleration->GetMode() == VAMode::ComponentAndDeviations)
                                                cout << " Sigma X: " << vtLds.m_pAcceleration->SigmaX << " Sigma Y: " << vtLds.m_pAcceleration->SigmaY << " Sigma Z: " << vtLds.m_pAcceleration->SigmaZ;
                                        else
                                                cout << " Rho_X_Y: " << vtLds.m_pAcceleration->Rho_X_Y << " Rho_X_Z: " << vtLds.m_pAcceleration->Rho_X_Z << " Rho_Y_Z: " << vtLds.m_pAcceleration->Rho_Y_Z;
                                cout << endl;
                        }

                        SafeArrayUnaccessData(vt.parray);
                }

        }
        break;
        case VChip_LDS:

                break;
        }
}


// Check if file exists
bool exists(const char* path) {
        ifstream f(path);
        return f.good();
}



// Read binary data from file
unsigned char* ReadBinData(const char* path, size_t& l)
{
        unsigned char * buffer;

        ifstream is;
        is.open(path, ios::in | ios::out | ios::binary | ios::ate);
        if (!is.is_open())
        {
                printf("File %s not found.", path);
                throw "open failed";
        }

        // get length of the file:
        is.seekg(0, ios::end);
        l = is.tellg();
        is.seekg(0, ios::beg);

        buffer = new unsigned char[l];

        is.read((char*)buffer, l);
        is.close();

        return buffer;
}

#pragma warning(pop)