Vehicle Researcher
7 years ago
parent
aee7d818e8
commit
7c1e812922
35 changed files with 1227 additions and 115 deletions
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:70323271320417698bb8640f18bbed920199fb61553ae85ff8a44a08a39e34b5 |
||||
size 2108896 |
||||
oid sha256:f1542ec60d6c18b0bef02f0dff85989814e99b9b35f331ae8416b6c0367fe368 |
||||
size 2116281 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:0ddd24ea3ab1349196d6413eed5432d5292d5912c641b6c3e76a46913b284080 |
||||
size 14337199 |
||||
oid sha256:8aa566ee67a720686f86bcde0eb4905d26735a6fa1d08459f254c8bddad95270 |
||||
size 17455432 |
||||
|
||||
@ -1 +1,3 @@ |
||||
gen |
||||
node_modules |
||||
package-lock.json |
||||
|
||||
@ -1,18 +0,0 @@ |
||||
using Cxx = import "c++.capnp"; |
||||
$Cxx.namespace("cereal"); |
||||
|
||||
using Java = import "java.capnp"; |
||||
$Java.package("ai.comma.openpilot.cereal"); |
||||
$Java.outerClassname("Map"); |
||||
|
||||
using Log = import "log.capnp"; |
||||
|
||||
@0xe1a6ab330ea7205f; |
||||
|
||||
struct MapdRequest { |
||||
pos @0 :Log.ECEFPoint; |
||||
} |
||||
|
||||
struct MapdResponse { |
||||
kfs @0 :List(Log.OrbKeyFrame); |
||||
} |
||||
@ -1 +1 @@ |
||||
#define COMMA_VERSION "0.4.4-release" |
||||
#define COMMA_VERSION "0.4.5-release" |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:900915b281d311e13e98ca6c0dcb16966f1127af00d1523b38c8a0fc029c97c1 |
||||
oid sha256:86a303c99ff57d29f0c3cc8656ae2cbd413a01894f89c718b5b9481c71b1ba4d |
||||
size 1416464 |
||||
|
||||
@ -0,0 +1,98 @@ |
||||
# CPU
|
||||
|
||||
CC = clang
|
||||
CXX = clang++
|
||||
|
||||
JSON_FLAGS = -I$(PHONELIBS)/json/src
|
||||
|
||||
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I.
|
||||
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I.
|
||||
LDFLAGS =
|
||||
|
||||
# profile
|
||||
# CXXFLAGS += -DTURBOCV_PROFILE=1
|
||||
|
||||
PHONELIBS = ../../phonelibs
|
||||
BASEDIR = ../..
|
||||
EXTERNAL = ../../external
|
||||
PYTHONLIBS =
|
||||
|
||||
UNAME_M := $(shell uname -m)
|
||||
|
||||
ifeq ($(UNAME_M),x86_64) |
||||
# computer
|
||||
|
||||
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
||||
ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \
|
||||
-l:libczmq.a -l:libzmq.a -lpthread
|
||||
|
||||
OPENCV_LIBS = -lopencv_core -lopencv_highgui -lopencv_features2d -lopencv_imgproc
|
||||
|
||||
CXXFLAGS += -fopenmp
|
||||
LDFLAGS += -lomp
|
||||
|
||||
else |
||||
# phone
|
||||
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
||||
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
|
||||
-l:libczmq.a -l:libzmq.a \
|
||||
-lgnustl_shared
|
||||
|
||||
OPENCV_FLAGS = -I$(PHONELIBS)/opencv/include
|
||||
OPENCV_LIBS = -Wl,--enable-new-dtags -Wl,-rpath,/usr/local/lib/python2.7/site-packages -L/usr/local/lib/python2.7/site-packages -l:cv2.so
|
||||
|
||||
endif |
||||
|
||||
.PHONY: all |
||||
all: orbd |
||||
|
||||
include ../common/cereal.mk |
||||
|
||||
DEP_OBJS = ../common/visionipc.o ../common/swaglog.o $(PHONELIBS)/json/src/json.o
|
||||
|
||||
orbd: orbd_dsp.o $(DEP_OBJS) calculator_stub.o freethedsp.o |
||||
@echo "[ LINK ] $@"
|
||||
$(CXX) -fPIC -o '$@' $^ \
|
||||
$(LDFLAGS) \
|
||||
$(ZMQ_LIBS) \
|
||||
$(CEREAL_LIBS) \
|
||||
-L/usr/lib \
|
||||
-L/system/vendor/lib64 \
|
||||
-ladsprpc \
|
||||
-lm -lz -llog
|
||||
|
||||
%.o: %.c |
||||
@echo "[ CC ] $@"
|
||||
$(CC) $(CFLAGS) \
|
||||
-I../ \
|
||||
-I../../ \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
orbd_dsp.o: orbd.cc |
||||
@echo "[ CXX ] $@"
|
||||
$(CXX) $(CXXFLAGS) \
|
||||
$(CEREAL_CXXFLAGS) \
|
||||
$(ZMQ_FLAGS) \
|
||||
$(OPENCV_FLAGS) \
|
||||
-DDSP \
|
||||
-I../ \
|
||||
-I../../ \
|
||||
-I../../../ \
|
||||
-I./include \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
freethedsp.o: dsp/freethedsp.c |
||||
@echo "[ CC ] $@"
|
||||
$(CC) $(CFLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
calculator_stub.o: dsp/gen/calculator_stub.c |
||||
@echo "[ CC ] $@"
|
||||
$(CC) $(CFLAGS) -I./include -c -o '$@' '$<'
|
||||
|
||||
-include internal.mk |
||||
|
||||
.PHONY: clean |
||||
clean: |
||||
rm -f *.o turbocv.so orbd test/turbocv_profile test/turbocv_test test/*.o *_lut.h
|
||||
|
||||
@ -0,0 +1,118 @@ |
||||
// freethedsp by geohot
|
||||
// (because the DSP should be free)
|
||||
// released under MIT License
|
||||
|
||||
// usage instructions:
|
||||
// 1. Compile an example from the Qualcomm Hexagon SDK
|
||||
// 2. Try to run it on your phone
|
||||
// 3. Be very sad when "adsprpc ... dlopen error: ... signature verify start failed for ..." appears in logcat
|
||||
// ...here is where people would give up before freethedsp
|
||||
// 4. Compile freethedsp with 'clang -shared freethedsp.c -o freethedsp.so' (or statically link it to your program)
|
||||
// 5. Run your program with 'LD_PRELOAD=./freethedsp.so ./<your_prog>'
|
||||
// 6. OMG THE DSP WORKS
|
||||
// 7. Be happy.
|
||||
|
||||
// *** patch may have to change for your phone ***
|
||||
|
||||
// this is patching /dsp/fastrpc_shell_0
|
||||
// correct if sha hash of fastrpc_shell_0 is "fbadc96848aefad99a95aa4edb560929dcdf78f8"
|
||||
// patch to return 0xFFFFFFFF from is_test_enabled instead of 0
|
||||
// your fastrpc_shell_0 may vary
|
||||
#define PATCH_ADDR 0x5200c |
||||
#define PATCH_OLD "\x40\x3f\x20\x50" |
||||
#define PATCH_NEW "\x40\x3f\x00\x5a" |
||||
#define PATCH_LEN (sizeof(PATCH_OLD)-1) |
||||
|
||||
// under 100 lines of code begins now
|
||||
#include <stdio.h> |
||||
#include <dlfcn.h> |
||||
#include <assert.h> |
||||
#include <stdlib.h> |
||||
#include <unistd.h> |
||||
|
||||
// ioctl stuff
|
||||
#define IOC_OUT 0x40000000 /* copy out parameters */ |
||||
#define IOC_IN 0x80000000 /* copy in parameters */ |
||||
#define IOC_INOUT (IOC_IN|IOC_OUT) |
||||
#define IOCPARM_MASK 0x1fff /* parameter length, at most 13 bits */ |
||||
|
||||
#define _IOC(inout,group,num,len) \ |
||||
(inout | ((len & IOCPARM_MASK) << 16) | ((group) << 8) | (num)) |
||||
#define _IOWR(g,n,t) _IOC(IOC_INOUT, (g), (n), sizeof(t)) |
||||
|
||||
// ion ioctls
|
||||
#include <linux/ion.h> |
||||
#define ION_IOC_MSM_MAGIC 'M' |
||||
#define ION_IOC_CLEAN_INV_CACHES _IOWR(ION_IOC_MSM_MAGIC, 2, \ |
||||
struct ion_flush_data) |
||||
|
||||
struct ion_flush_data { |
||||
ion_user_handle_t handle; |
||||
int fd; |
||||
void *vaddr; |
||||
unsigned int offset; |
||||
unsigned int length; |
||||
}; |
||||
|
||||
// fastrpc ioctls
|
||||
#define FASTRPC_IOCTL_INIT _IOWR('R', 6, struct fastrpc_ioctl_init) |
||||
|
||||
struct fastrpc_ioctl_init { |
||||
uint32_t flags; /* one of FASTRPC_INIT_* macros */ |
||||
uintptr_t __user file; /* pointer to elf file */ |
||||
int32_t filelen; /* elf file length */ |
||||
int32_t filefd; /* ION fd for the file */ |
||||
uintptr_t __user mem; /* mem for the PD */ |
||||
int32_t memlen; /* mem length */ |
||||
int32_t memfd; /* ION fd for the mem */ |
||||
}; |
||||
|
||||
int ioctl(int fd, unsigned long request, void *arg) { |
||||
static void *handle = NULL; |
||||
static int (*orig_ioctl)(int, int, void*); |
||||
|
||||
if (handle == NULL) { |
||||
handle = dlopen("/system/lib64/libc.so", RTLD_LAZY); |
||||
assert(handle != NULL); |
||||
orig_ioctl = dlsym(handle, "ioctl"); |
||||
} |
||||
|
||||
int ret = orig_ioctl(fd, request, arg); |
||||
|
||||
// carefully modify this one
|
||||
if (request == FASTRPC_IOCTL_INIT) { |
||||
struct fastrpc_ioctl_init *init = (struct fastrpc_ioctl_init *)arg; |
||||
|
||||
// confirm patch is correct and do the patch
|
||||
assert(memcmp((void*)(init->mem+PATCH_ADDR), PATCH_OLD, 4) == 0); |
||||
memcpy((void*)(init->mem+PATCH_ADDR), PATCH_NEW, 4); |
||||
|
||||
// flush cache
|
||||
int ionfd = open("/dev/ion", O_RDONLY); |
||||
assert(ionfd > 0); |
||||
|
||||
struct ion_fd_data fd_data; |
||||
fd_data.fd = init->memfd; |
||||
int ret = ioctl(ionfd, ION_IOC_IMPORT, &fd_data); |
||||
assert(ret == 0); |
||||
|
||||
struct ion_flush_data flush_data; |
||||
flush_data.handle = fd_data.handle; |
||||
flush_data.vaddr = (void*)init->mem; |
||||
flush_data.offset = PATCH_ADDR; |
||||
flush_data.length = PATCH_LEN; |
||||
ret = ioctl(ionfd, ION_IOC_CLEAN_INV_CACHES, &flush_data); |
||||
assert(ret == 0); |
||||
|
||||
struct ion_handle_data handle_data; |
||||
handle_data.handle = fd_data.handle; |
||||
ret = ioctl(ionfd, ION_IOC_FREE, &handle_data); |
||||
assert(ret == 0); |
||||
|
||||
// cleanup
|
||||
close(ionfd); |
||||
} |
||||
|
||||
return ret; |
||||
} |
||||
|
||||
@ -0,0 +1,39 @@ |
||||
#ifndef _CALCULATOR_H |
||||
#define _CALCULATOR_H |
||||
|
||||
#include <stdint.h> |
||||
typedef uint8_t uint8; |
||||
typedef uint32_t uint32; |
||||
|
||||
#ifndef __QAIC_HEADER |
||||
#define __QAIC_HEADER(ff) ff |
||||
#endif //__QAIC_HEADER
|
||||
|
||||
#ifndef __QAIC_HEADER_EXPORT |
||||
#define __QAIC_HEADER_EXPORT |
||||
#endif // __QAIC_HEADER_EXPORT
|
||||
|
||||
#ifndef __QAIC_HEADER_ATTRIBUTE |
||||
#define __QAIC_HEADER_ATTRIBUTE |
||||
#endif // __QAIC_HEADER_ATTRIBUTE
|
||||
|
||||
#ifndef __QAIC_IMPL |
||||
#define __QAIC_IMPL(ff) ff |
||||
#endif //__QAIC_IMPL
|
||||
|
||||
#ifndef __QAIC_IMPL_EXPORT |
||||
#define __QAIC_IMPL_EXPORT |
||||
#endif // __QAIC_IMPL_EXPORT
|
||||
|
||||
#ifndef __QAIC_IMPL_ATTRIBUTE |
||||
#define __QAIC_IMPL_ATTRIBUTE |
||||
#endif // __QAIC_IMPL_ATTRIBUTE
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_init)(uint32* leet) __QAIC_HEADER_ATTRIBUTE; |
||||
__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_HEADER_ATTRIBUTE; |
||||
#ifdef __cplusplus |
||||
} |
||||
#endif |
||||
#endif //_CALCULATOR_H
|
||||
@ -0,0 +1,613 @@ |
||||
#ifndef _CALCULATOR_STUB_H |
||||
#define _CALCULATOR_STUB_H |
||||
#include "calculator.h" |
||||
|
||||
// remote.h
|
||||
#include <stdint.h> |
||||
#include <sys/types.h> |
||||
|
||||
typedef uint32_t remote_handle; |
||||
typedef uint64_t remote_handle64; |
||||
|
||||
typedef struct { |
||||
void *pv; |
||||
size_t nLen; |
||||
} remote_buf; |
||||
|
||||
typedef struct { |
||||
int32_t fd; |
||||
uint32_t offset; |
||||
} remote_dma_handle; |
||||
|
||||
typedef union { |
||||
remote_buf buf; |
||||
remote_handle h; |
||||
remote_handle64 h64; |
||||
remote_dma_handle dma; |
||||
} remote_arg; |
||||
|
||||
int remote_handle_open(const char* name, remote_handle *ph); |
||||
int remote_handle_invoke(remote_handle h, uint32_t dwScalars, remote_arg *pra); |
||||
int remote_handle_close(remote_handle h); |
||||
|
||||
#define REMOTE_SCALARS_MAKEX(nAttr,nMethod,nIn,nOut,noIn,noOut) \ |
||||
((((uint32_t) (nAttr) & 0x7) << 29) | \
|
||||
(((uint32_t) (nMethod) & 0x1f) << 24) | \
|
||||
(((uint32_t) (nIn) & 0xff) << 16) | \
|
||||
(((uint32_t) (nOut) & 0xff) << 8) | \
|
||||
(((uint32_t) (noIn) & 0x0f) << 4) | \
|
||||
((uint32_t) (noOut) & 0x0f)) |
||||
|
||||
#ifndef _QAIC_ENV_H |
||||
#define _QAIC_ENV_H |
||||
|
||||
#ifdef __GNUC__ |
||||
#ifdef __clang__ |
||||
#pragma GCC diagnostic ignored "-Wunknown-pragmas" |
||||
#else |
||||
#pragma GCC diagnostic ignored "-Wpragmas" |
||||
#endif |
||||
#pragma GCC diagnostic ignored "-Wuninitialized" |
||||
#pragma GCC diagnostic ignored "-Wunused-parameter" |
||||
#pragma GCC diagnostic ignored "-Wunused-function" |
||||
#endif |
||||
|
||||
#ifndef _ATTRIBUTE_UNUSED |
||||
|
||||
#ifdef _WIN32 |
||||
#define _ATTRIBUTE_UNUSED |
||||
#else |
||||
#define _ATTRIBUTE_UNUSED __attribute__ ((unused)) |
||||
#endif |
||||
|
||||
#endif // _ATTRIBUTE_UNUSED
|
||||
|
||||
#ifndef __QAIC_REMOTE |
||||
#define __QAIC_REMOTE(ff) ff |
||||
#endif //__QAIC_REMOTE
|
||||
|
||||
#ifndef __QAIC_HEADER |
||||
#define __QAIC_HEADER(ff) ff |
||||
#endif //__QAIC_HEADER
|
||||
|
||||
#ifndef __QAIC_HEADER_EXPORT |
||||
#define __QAIC_HEADER_EXPORT |
||||
#endif // __QAIC_HEADER_EXPORT
|
||||
|
||||
#ifndef __QAIC_HEADER_ATTRIBUTE |
||||
#define __QAIC_HEADER_ATTRIBUTE |
||||
#endif // __QAIC_HEADER_ATTRIBUTE
|
||||
|
||||
#ifndef __QAIC_IMPL |
||||
#define __QAIC_IMPL(ff) ff |
||||
#endif //__QAIC_IMPL
|
||||
|
||||
#ifndef __QAIC_IMPL_EXPORT |
||||
#define __QAIC_IMPL_EXPORT |
||||
#endif // __QAIC_IMPL_EXPORT
|
||||
|
||||
#ifndef __QAIC_IMPL_ATTRIBUTE |
||||
#define __QAIC_IMPL_ATTRIBUTE |
||||
#endif // __QAIC_IMPL_ATTRIBUTE
|
||||
|
||||
#ifndef __QAIC_STUB |
||||
#define __QAIC_STUB(ff) ff |
||||
#endif //__QAIC_STUB
|
||||
|
||||
#ifndef __QAIC_STUB_EXPORT |
||||
#define __QAIC_STUB_EXPORT |
||||
#endif // __QAIC_STUB_EXPORT
|
||||
|
||||
#ifndef __QAIC_STUB_ATTRIBUTE |
||||
#define __QAIC_STUB_ATTRIBUTE |
||||
#endif // __QAIC_STUB_ATTRIBUTE
|
||||
|
||||
#ifndef __QAIC_SKEL |
||||
#define __QAIC_SKEL(ff) ff |
||||
#endif //__QAIC_SKEL__
|
||||
|
||||
#ifndef __QAIC_SKEL_EXPORT |
||||
#define __QAIC_SKEL_EXPORT |
||||
#endif // __QAIC_SKEL_EXPORT
|
||||
|
||||
#ifndef __QAIC_SKEL_ATTRIBUTE |
||||
#define __QAIC_SKEL_ATTRIBUTE |
||||
#endif // __QAIC_SKEL_ATTRIBUTE
|
||||
|
||||
#ifdef __QAIC_DEBUG__ |
||||
#ifndef __QAIC_DBG_PRINTF__ |
||||
#include <stdio.h> |
||||
#define __QAIC_DBG_PRINTF__( ee ) do { printf ee ; } while(0) |
||||
#endif |
||||
#else |
||||
#define __QAIC_DBG_PRINTF__( ee ) (void)0 |
||||
#endif |
||||
|
||||
|
||||
#define _OFFSET(src, sof) ((void*)(((char*)(src)) + (sof))) |
||||
|
||||
#define _COPY(dst, dof, src, sof, sz) \ |
||||
do {\
|
||||
struct __copy { \
|
||||
char ar[sz]; \
|
||||
};\
|
||||
*(struct __copy*)_OFFSET(dst, dof) = *(struct __copy*)_OFFSET(src, sof);\
|
||||
} while (0) |
||||
|
||||
#define _COPYIF(dst, dof, src, sof, sz) \ |
||||
do {\
|
||||
if(_OFFSET(dst, dof) != _OFFSET(src, sof)) {\
|
||||
_COPY(dst, dof, src, sof, sz); \
|
||||
} \
|
||||
} while (0) |
||||
|
||||
_ATTRIBUTE_UNUSED |
||||
static __inline void _qaic_memmove(void* dst, void* src, int size) { |
||||
int i; |
||||
for(i = 0; i < size; ++i) { |
||||
((char*)dst)[i] = ((char*)src)[i]; |
||||
} |
||||
} |
||||
|
||||
#define _MEMMOVEIF(dst, src, sz) \ |
||||
do {\
|
||||
if(dst != src) {\
|
||||
_qaic_memmove(dst, src, sz);\
|
||||
} \
|
||||
} while (0) |
||||
|
||||
|
||||
#define _ASSIGN(dst, src, sof) \ |
||||
do {\
|
||||
dst = OFFSET(src, sof); \
|
||||
} while (0) |
||||
|
||||
#define _STD_STRLEN_IF(str) (str == 0 ? 0 : strlen(str)) |
||||
|
||||
#define AEE_SUCCESS 0 |
||||
#define AEE_EOFFSET 0x80000400 |
||||
#define AEE_EBADPARM (AEE_EOFFSET + 0x00E) |
||||
|
||||
#define _TRY(ee, func) \ |
||||
do { \
|
||||
if (AEE_SUCCESS != ((ee) = func)) {\
|
||||
__QAIC_DBG_PRINTF__((__FILE__ ":%d:error:%d:%s\n", __LINE__, (int)(ee),#func));\
|
||||
goto ee##bail;\
|
||||
} \
|
||||
} while (0) |
||||
|
||||
#define _CATCH(exception) exception##bail: if (exception != AEE_SUCCESS) |
||||
|
||||
#define _ASSERT(nErr, ff) _TRY(nErr, 0 == (ff) ? AEE_EBADPARM : AEE_SUCCESS) |
||||
|
||||
#ifdef __QAIC_DEBUG__ |
||||
#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, __FILE_LINE__, size, alignment, (void**)&pv)) |
||||
#else |
||||
#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, 0, size, alignment, (void**)&pv)) |
||||
#endif |
||||
|
||||
|
||||
#endif // _QAIC_ENV_H
|
||||
|
||||
#ifndef _ALLOCATOR_H |
||||
#define _ALLOCATOR_H |
||||
|
||||
#include <stdlib.h> |
||||
#include <stdint.h> |
||||
|
||||
typedef struct _heap _heap; |
||||
struct _heap { |
||||
_heap* pPrev; |
||||
const char* loc; |
||||
uint64_t buf; |
||||
}; |
||||
|
||||
typedef struct _allocator { |
||||
_heap* pheap; |
||||
uint8_t* stack; |
||||
uint8_t* stackEnd; |
||||
int nSize; |
||||
} _allocator; |
||||
|
||||
_ATTRIBUTE_UNUSED |
||||
static __inline int _heap_alloc(_heap** ppa, const char* loc, int size, void** ppbuf) { |
||||
_heap* pn = 0; |
||||
pn = malloc(size + sizeof(_heap) - sizeof(uint64_t)); |
||||
if(pn != 0) { |
||||
pn->pPrev = *ppa; |
||||
pn->loc = loc; |
||||
*ppa = pn; |
||||
*ppbuf = (void*)&(pn->buf); |
||||
return 0; |
||||
} else { |
||||
return -1; |
||||
} |
||||
} |
||||
#define _ALIGN_SIZE(x, y) (((x) + (y-1)) & ~(y-1)) |
||||
|
||||
_ATTRIBUTE_UNUSED |
||||
static __inline int _allocator_alloc(_allocator* me, |
||||
const char* loc, |
||||
int size, |
||||
unsigned int al, |
||||
void** ppbuf) { |
||||
if(size < 0) { |
||||
return -1; |
||||
} else if (size == 0) { |
||||
*ppbuf = 0; |
||||
return 0; |
||||
} |
||||
if((_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size) < (uintptr_t)me->stack + me->nSize) { |
||||
*ppbuf = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al); |
||||
me->stackEnd = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size; |
||||
return 0; |
||||
} else { |
||||
return _heap_alloc(&me->pheap, loc, size, ppbuf); |
||||
} |
||||
} |
||||
|
||||
_ATTRIBUTE_UNUSED |
||||
static __inline void _allocator_deinit(_allocator* me) { |
||||
_heap* pa = me->pheap; |
||||
while(pa != 0) { |
||||
_heap* pn = pa; |
||||
const char* loc = pn->loc; |
||||
(void)loc; |
||||
pa = pn->pPrev; |
||||
free(pn); |
||||
} |
||||
} |
||||
|
||||
_ATTRIBUTE_UNUSED |
||||
static __inline void _allocator_init(_allocator* me, uint8_t* stack, int stackSize) { |
||||
me->stack = stack; |
||||
me->stackEnd = stack + stackSize; |
||||
me->nSize = stackSize; |
||||
me->pheap = 0; |
||||
} |
||||
|
||||
|
||||
#endif // _ALLOCATOR_H
|
||||
|
||||
#ifndef SLIM_H |
||||
#define SLIM_H |
||||
|
||||
#include <stdint.h> |
||||
|
||||
//a C data structure for the idl types that can be used to implement
|
||||
//static and dynamic language bindings fairly efficiently.
|
||||
//
|
||||
//the goal is to have a minimal ROM and RAM footprint and without
|
||||
//doing too many allocations. A good way to package these things seemed
|
||||
//like the module boundary, so all the idls within one module can share
|
||||
//all the type references.
|
||||
|
||||
|
||||
#define PARAMETER_IN 0x0 |
||||
#define PARAMETER_OUT 0x1 |
||||
#define PARAMETER_INOUT 0x2 |
||||
#define PARAMETER_ROUT 0x3 |
||||
#define PARAMETER_INROUT 0x4 |
||||
|
||||
//the types that we get from idl
|
||||
#define TYPE_OBJECT 0x0 |
||||
#define TYPE_INTERFACE 0x1 |
||||
#define TYPE_PRIMITIVE 0x2 |
||||
#define TYPE_ENUM 0x3 |
||||
#define TYPE_STRING 0x4 |
||||
#define TYPE_WSTRING 0x5 |
||||
#define TYPE_STRUCTURE 0x6 |
||||
#define TYPE_UNION 0x7 |
||||
#define TYPE_ARRAY 0x8 |
||||
#define TYPE_SEQUENCE 0x9 |
||||
|
||||
//these require the pack/unpack to recurse
|
||||
//so it's a hint to those languages that can optimize in cases where
|
||||
//recursion isn't necessary.
|
||||
#define TYPE_COMPLEX_STRUCTURE (0x10 | TYPE_STRUCTURE) |
||||
#define TYPE_COMPLEX_UNION (0x10 | TYPE_UNION) |
||||
#define TYPE_COMPLEX_ARRAY (0x10 | TYPE_ARRAY) |
||||
#define TYPE_COMPLEX_SEQUENCE (0x10 | TYPE_SEQUENCE) |
||||
|
||||
|
||||
typedef struct Type Type; |
||||
|
||||
#define INHERIT_TYPE\ |
||||
int32_t nativeSize; /*in the simple case its the same as wire size and alignment*/\
|
||||
union {\
|
||||
struct {\
|
||||
const uintptr_t p1;\
|
||||
const uintptr_t p2;\
|
||||
} _cast;\
|
||||
struct {\
|
||||
uint32_t iid;\
|
||||
uint32_t bNotNil;\
|
||||
} object;\
|
||||
struct {\
|
||||
const Type *arrayType;\
|
||||
int32_t nItems;\
|
||||
} array;\
|
||||
struct {\
|
||||
const Type *seqType;\
|
||||
int32_t nMaxLen;\
|
||||
} seqSimple; \
|
||||
struct {\
|
||||
uint32_t bFloating;\
|
||||
uint32_t bSigned;\
|
||||
} prim; \
|
||||
const SequenceType* seqComplex;\
|
||||
const UnionType *unionType;\
|
||||
const StructType *structType;\
|
||||
int32_t stringMaxLen;\
|
||||
uint8_t bInterfaceNotNil;\
|
||||
} param;\
|
||||
uint8_t type;\
|
||||
uint8_t nativeAlignment\
|
||||
|
||||
typedef struct UnionType UnionType; |
||||
typedef struct StructType StructType; |
||||
typedef struct SequenceType SequenceType; |
||||
struct Type { |
||||
INHERIT_TYPE; |
||||
}; |
||||
|
||||
struct SequenceType { |
||||
const Type * seqType; |
||||
uint32_t nMaxLen; |
||||
uint32_t inSize; |
||||
uint32_t routSizePrimIn; |
||||
uint32_t routSizePrimROut; |
||||
}; |
||||
|
||||
//byte offset from the start of the case values for
|
||||
//this unions case value array. it MUST be aligned
|
||||
//at the alignment requrements for the descriptor
|
||||
//
|
||||
//if negative it means that the unions cases are
|
||||
//simple enumerators, so the value read from the descriptor
|
||||
//can be used directly to find the correct case
|
||||
typedef union CaseValuePtr CaseValuePtr; |
||||
union CaseValuePtr { |
||||
const uint8_t* value8s; |
||||
const uint16_t* value16s; |
||||
const uint32_t* value32s; |
||||
const uint64_t* value64s; |
||||
}; |
||||
|
||||
//these are only used in complex cases
|
||||
//so I pulled them out of the type definition as references to make
|
||||
//the type smaller
|
||||
struct UnionType { |
||||
const Type *descriptor; |
||||
uint32_t nCases; |
||||
const CaseValuePtr caseValues; |
||||
const Type * const *cases; |
||||
int32_t inSize; |
||||
int32_t routSizePrimIn; |
||||
int32_t routSizePrimROut; |
||||
uint8_t inAlignment; |
||||
uint8_t routAlignmentPrimIn; |
||||
uint8_t routAlignmentPrimROut; |
||||
uint8_t inCaseAlignment; |
||||
uint8_t routCaseAlignmentPrimIn; |
||||
uint8_t routCaseAlignmentPrimROut; |
||||
uint8_t nativeCaseAlignment; |
||||
uint8_t bDefaultCase; |
||||
}; |
||||
|
||||
struct StructType { |
||||
uint32_t nMembers; |
||||
const Type * const *members; |
||||
int32_t inSize; |
||||
int32_t routSizePrimIn; |
||||
int32_t routSizePrimROut; |
||||
uint8_t inAlignment; |
||||
uint8_t routAlignmentPrimIn; |
||||
uint8_t routAlignmentPrimROut; |
||||
}; |
||||
|
||||
typedef struct Parameter Parameter; |
||||
struct Parameter { |
||||
INHERIT_TYPE; |
||||
uint8_t mode; |
||||
uint8_t bNotNil; |
||||
}; |
||||
|
||||
#define SLIM_IFPTR32(is32,is64) (sizeof(uintptr_t) == 4 ? (is32) : (is64)) |
||||
#define SLIM_SCALARS_IS_DYNAMIC(u) (((u) & 0x00ffffff) == 0x00ffffff) |
||||
|
||||
typedef struct Method Method; |
||||
struct Method { |
||||
uint32_t uScalars; //no method index
|
||||
int32_t primInSize; |
||||
int32_t primROutSize; |
||||
int maxArgs; |
||||
int numParams; |
||||
const Parameter * const *params; |
||||
uint8_t primInAlignment; |
||||
uint8_t primROutAlignment; |
||||
}; |
||||
|
||||
typedef struct Interface Interface; |
||||
|
||||
struct Interface { |
||||
int nMethods; |
||||
const Method * const *methodArray; |
||||
int nIIds; |
||||
const uint32_t *iids; |
||||
const uint16_t* methodStringArray; |
||||
const uint16_t* methodStrings; |
||||
const char* strings; |
||||
}; |
||||
|
||||
|
||||
#endif //SLIM_H
|
||||
|
||||
|
||||
#ifndef _CALCULATOR_SLIM_H |
||||
#define _CALCULATOR_SLIM_H |
||||
|
||||
// remote.h
|
||||
|
||||
#include <stdint.h> |
||||
|
||||
#ifndef __QAIC_SLIM |
||||
#define __QAIC_SLIM(ff) ff |
||||
#endif |
||||
#ifndef __QAIC_SLIM_EXPORT |
||||
#define __QAIC_SLIM_EXPORT |
||||
#endif |
||||
|
||||
static const Type types[1]; |
||||
static const Type types[1] = {{0x1,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x1}}; |
||||
static const Parameter parameters[3] = {{0x4,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x4,3,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),0,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),3,0}}; |
||||
static const Parameter* const parameterArrays[3] = {(&(parameters[1])),(&(parameters[2])),(&(parameters[0]))}; |
||||
static const Method methods[2] = {{REMOTE_SCALARS_MAKEX(0,0,0x0,0x1,0x0,0x0),0x0,0x4,1,1,(&(parameterArrays[2])),0x1,0x4},{REMOTE_SCALARS_MAKEX(0,0,0x2,0x1,0x0,0x0),0x8,0x0,5,2,(&(parameterArrays[0])),0x4,0x1}}; |
||||
static const Method* const methodArrays[2] = {&(methods[0]),&(methods[1])}; |
||||
static const char strings[41] = "extract_and_match\0features\0leet\0init\0img\0"; |
||||
static const uint16_t methodStrings[5] = {0,37,18,32,27}; |
||||
static const uint16_t methodStringsArrays[2] = {3,0}; |
||||
__QAIC_SLIM_EXPORT const Interface __QAIC_SLIM(calculator_slim) = {2,&(methodArrays[0]),0,0,&(methodStringsArrays [0]),methodStrings,strings}; |
||||
#endif //_CALCULATOR_SLIM_H
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
#ifndef _const_calculator_handle |
||||
#define _const_calculator_handle ((remote_handle)-1) |
||||
#endif //_const_calculator_handle
|
||||
|
||||
static void _calculator_pls_dtor(void* data) { |
||||
remote_handle* ph = (remote_handle*)data; |
||||
if(_const_calculator_handle != *ph) { |
||||
(void)__QAIC_REMOTE(remote_handle_close)(*ph); |
||||
*ph = _const_calculator_handle; |
||||
} |
||||
} |
||||
|
||||
static int _calculator_pls_ctor(void* ctx, void* data) { |
||||
remote_handle* ph = (remote_handle*)data; |
||||
*ph = _const_calculator_handle; |
||||
if(*ph == (remote_handle)-1) { |
||||
return __QAIC_REMOTE(remote_handle_open)((const char*)ctx, ph); |
||||
} |
||||
return 0; |
||||
} |
||||
|
||||
#if (defined __qdsp6__) || (defined __hexagon__) |
||||
#pragma weak adsp_pls_add_lookup |
||||
extern int adsp_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo); |
||||
#pragma weak HAP_pls_add_lookup |
||||
extern int HAP_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo); |
||||
|
||||
__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) { |
||||
remote_handle* ph; |
||||
if(adsp_pls_add_lookup) { |
||||
if(0 == adsp_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) { |
||||
return *ph; |
||||
} |
||||
return (remote_handle)-1; |
||||
} else if(HAP_pls_add_lookup) { |
||||
if(0 == HAP_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) { |
||||
return *ph; |
||||
} |
||||
return (remote_handle)-1; |
||||
} |
||||
return(remote_handle)-1; |
||||
} |
||||
|
||||
#else //__qdsp6__ || __hexagon__
|
||||
|
||||
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare); |
||||
|
||||
#ifdef _WIN32 |
||||
#include "Windows.h" |
||||
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) { |
||||
return (uint32_t)InterlockedCompareExchange((volatile LONG*)puDest, (LONG)uExchange, (LONG)uCompare); |
||||
} |
||||
#elif __GNUC__ |
||||
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) { |
||||
return __sync_val_compare_and_swap(puDest, uCompare, uExchange); |
||||
} |
||||
#endif //_WIN32
|
||||
|
||||
|
||||
__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) { |
||||
static remote_handle handle = _const_calculator_handle; |
||||
if((remote_handle)-1 != handle) { |
||||
return handle; |
||||
} else { |
||||
remote_handle tmp; |
||||
int nErr = _calculator_pls_ctor("calculator", (void*)&tmp); |
||||
if(nErr) { |
||||
return (remote_handle)-1; |
||||
} |
||||
if(((remote_handle)-1 != handle) || ((remote_handle)-1 != (remote_handle)_calculator_atomic_CompareAndExchange((uint32_t*)&handle, (uint32_t)tmp, (uint32_t)-1))) { |
||||
_calculator_pls_dtor(&tmp); |
||||
} |
||||
return handle; |
||||
} |
||||
} |
||||
|
||||
#endif //__qdsp6__
|
||||
|
||||
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_skel_invoke)(uint32_t _sc, remote_arg* _pra) __QAIC_STUB_ATTRIBUTE { |
||||
return __QAIC_REMOTE(remote_handle_invoke)(_calculator_handle(), _sc, _pra); |
||||
} |
||||
|
||||
#ifdef __cplusplus |
||||
} |
||||
#endif |
||||
|
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
extern int remote_register_dma_handle(int, uint32_t); |
||||
static __inline int _stub_method(remote_handle _handle, uint32_t _mid, uint32_t _rout0[1]) { |
||||
int _numIn[1]; |
||||
remote_arg _pra[1]; |
||||
uint32_t _primROut[1]; |
||||
int _nErr = 0; |
||||
_numIn[0] = 0; |
||||
_pra[(_numIn[0] + 0)].buf.pv = (void*)_primROut; |
||||
_pra[(_numIn[0] + 0)].buf.nLen = sizeof(_primROut); |
||||
_TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 0, 1, 0, 0), _pra)); |
||||
_COPY(_rout0, 0, _primROut, 0, 4); |
||||
_CATCH(_nErr) {} |
||||
return _nErr; |
||||
} |
||||
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_init)(uint32* leet) __QAIC_STUB_ATTRIBUTE { |
||||
uint32_t _mid = 0; |
||||
return _stub_method(_calculator_handle(), _mid, (uint32_t*)leet); |
||||
} |
||||
static __inline int _stub_method_1(remote_handle _handle, uint32_t _mid, char* _in0[1], uint32_t _in0Len[1], char* _rout1[1], uint32_t _rout1Len[1]) { |
||||
int _numIn[1]; |
||||
remote_arg _pra[3]; |
||||
uint32_t _primIn[2]; |
||||
remote_arg* _praIn; |
||||
remote_arg* _praROut; |
||||
int _nErr = 0; |
||||
_numIn[0] = 1; |
||||
_pra[0].buf.pv = (void*)_primIn; |
||||
_pra[0].buf.nLen = sizeof(_primIn); |
||||
_COPY(_primIn, 0, _in0Len, 0, 4); |
||||
_praIn = (_pra + 1); |
||||
_praIn[0].buf.pv = _in0[0]; |
||||
_praIn[0].buf.nLen = (1 * _in0Len[0]); |
||||
_COPY(_primIn, 4, _rout1Len, 0, 4); |
||||
_praROut = (_praIn + _numIn[0] + 0); |
||||
_praROut[0].buf.pv = _rout1[0]; |
||||
_praROut[0].buf.nLen = (1 * _rout1Len[0]); |
||||
_TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 2, 1, 0, 0), _pra)); |
||||
_CATCH(_nErr) {} |
||||
return _nErr; |
||||
} |
||||
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_STUB_ATTRIBUTE { |
||||
uint32_t _mid = 1; |
||||
return _stub_method_1(_calculator_handle(), _mid, (char**)&img, (uint32_t*)&imgLen, (char**)&features, (uint32_t*)&featuresLen); |
||||
} |
||||
#ifdef __cplusplus |
||||
} |
||||
#endif |
||||
#endif //_CALCULATOR_STUB_H
|
||||
@ -0,0 +1,37 @@ |
||||
#ifndef EXTRACTOR_H |
||||
#define EXTRACTOR_H |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
#include <stdint.h> |
||||
|
||||
#define ORBD_KEYPOINTS 3000 |
||||
#define ORBD_DESCRIPTOR_LENGTH 32 |
||||
#define ORBD_HEIGHT 874 |
||||
#define ORBD_WIDTH 1164 |
||||
|
||||
// matches OrbFeatures from log.capnp
|
||||
struct orb_features { |
||||
// align this
|
||||
uint16_t n_corners; |
||||
uint16_t xy[ORBD_KEYPOINTS][2]; |
||||
uint8_t octave[ORBD_KEYPOINTS]; |
||||
uint8_t des[ORBD_KEYPOINTS][ORBD_DESCRIPTOR_LENGTH]; |
||||
int16_t matches[ORBD_KEYPOINTS]; |
||||
}; |
||||
|
||||
// forward declare this
|
||||
struct pyramid; |
||||
|
||||
// manage the pyramids in extractor.c
|
||||
void init_gpyrs(); |
||||
int extract_and_match_gpyrs(const uint8_t *img, struct orb_features *); |
||||
int extract_and_match(const uint8_t *img, struct pyramid *pyrs, struct pyramid *prev_pyrs, struct orb_features *); |
||||
|
||||
#ifdef __cplusplus |
||||
} |
||||
#endif |
||||
|
||||
#endif // EXTRACTOR_H
|
||||
@ -0,0 +1,174 @@ |
||||
#include <stdio.h> |
||||
#include <stdlib.h> |
||||
#include <signal.h> |
||||
#include <unistd.h> |
||||
#include <stdint.h> |
||||
#include <assert.h> |
||||
|
||||
#include "common/visionipc.h" |
||||
#include "common/swaglog.h" |
||||
|
||||
#include "extractor.h" |
||||
|
||||
#ifdef DSP |
||||
#include "dsp/gen/calculator.h" |
||||
#else |
||||
#include "turbocv.h" |
||||
#endif |
||||
|
||||
#include <zmq.h> |
||||
#include <capnp/serialize.h> |
||||
#include "cereal/gen/cpp/log.capnp.h" |
||||
|
||||
#ifndef PATH_MAX |
||||
#include <linux/limits.h> |
||||
#endif |
||||
|
||||
volatile int do_exit = 0; |
||||
|
||||
static void set_do_exit(int sig) { |
||||
do_exit = 1; |
||||
} |
||||
|
||||
int main(int argc, char *argv[]) { |
||||
int err; |
||||
printf("starting orbd\n"); |
||||
|
||||
#ifdef DSP |
||||
char my_path[PATH_MAX+1]; |
||||
readlink("/proc/self/exe", my_path, PATH_MAX); |
||||
my_path[strlen(my_path)-4] = '\0'; |
||||
LOGW("running from %s", my_path); |
||||
|
||||
char adsp_path[PATH_MAX+1]; |
||||
snprintf(adsp_path, PATH_MAX, "ADSP_LIBRARY_PATH=%s/dsp/gen;/dsp;/system/lib/rfsa/adsp;/system/vendor/lib/rfsa/adsp", my_path); |
||||
putenv(adsp_path); |
||||
|
||||
uint32_t test_leet = 0;
|
||||
assert(calculator_init(&test_leet) == 0); |
||||
assert(test_leet == 0x1337); |
||||
#else |
||||
init_gpyrs(); |
||||
#endif |
||||
|
||||
signal(SIGINT, (sighandler_t) set_do_exit); |
||||
signal(SIGTERM, (sighandler_t) set_do_exit); |
||||
|
||||
void *ctx = zmq_ctx_new(); |
||||
|
||||
void *orb_features_sock = zmq_socket(ctx, ZMQ_PUB); |
||||
assert(orb_features_sock); |
||||
zmq_bind(orb_features_sock, "tcp://*:8058"); |
||||
|
||||
void *orb_features_summary_sock = zmq_socket(ctx, ZMQ_PUB); |
||||
assert(orb_features_summary_sock); |
||||
zmq_bind(orb_features_summary_sock, "tcp://*:8062"); |
||||
|
||||
struct orb_features *features = (struct orb_features *)malloc(sizeof(struct orb_features)); |
||||
int last_frame_id = 0; |
||||
|
||||
VisionStream stream; |
||||
while (!do_exit) { |
||||
VisionStreamBufs buf_info; |
||||
err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info); |
||||
if (err) { |
||||
printf("visionstream connect fail\n"); |
||||
usleep(100000); |
||||
continue; |
||||
} |
||||
uint64_t timestamp_last_eof = 0; |
||||
while (!do_exit) { |
||||
VIPCBuf *buf; |
||||
VIPCBufExtra extra; |
||||
buf = visionstream_get(&stream, &extra); |
||||
if (buf == NULL) { |
||||
printf("visionstream get failed\n"); |
||||
break; |
||||
} |
||||
|
||||
uint64_t start = nanos_since_boot(); |
||||
#ifdef DSP |
||||
int ret = calculator_extract_and_match((uint8_t *)buf->addr, ORBD_HEIGHT*ORBD_WIDTH, (uint8_t *)features, sizeof(struct orb_features)); |
||||
#else |
||||
int ret = extract_and_match_gpyrs((uint8_t *) buf->addr, features); |
||||
#endif |
||||
uint64_t end = nanos_since_boot(); |
||||
LOGD("total(%d): %6.2f ms to get %4d features on %d", ret, (end-start)/1000000.0, features->n_corners, extra.frame_id); |
||||
|
||||
if (last_frame_id+1 != extra.frame_id) { |
||||
LOGW("dropped frame!"); |
||||
} |
||||
|
||||
last_frame_id = extra.frame_id; |
||||
|
||||
if (timestamp_last_eof == 0) { |
||||
timestamp_last_eof = extra.timestamp_eof; |
||||
continue; |
||||
} |
||||
|
||||
int match_count = 0; |
||||
|
||||
// *** send OrbFeatures ***
|
||||
{ |
||||
// create capnp message
|
||||
capnp::MallocMessageBuilder msg; |
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
||||
event.setLogMonoTime(nanos_since_boot()); |
||||
|
||||
auto orb_features = event.initOrbFeatures(); |
||||
|
||||
// set timestamps
|
||||
orb_features.setTimestampEof(extra.timestamp_eof); |
||||
orb_features.setTimestampLastEof(timestamp_last_eof); |
||||
|
||||
// init descriptors for send
|
||||
kj::ArrayPtr<capnp::byte> descriptorsPtr = kj::arrayPtr((uint8_t *)features->des, ORBD_DESCRIPTOR_LENGTH * features->n_corners); |
||||
orb_features.setDescriptors(descriptorsPtr); |
||||
|
||||
auto xs = orb_features.initXs(features->n_corners); |
||||
auto ys = orb_features.initYs(features->n_corners); |
||||
auto octaves = orb_features.initOctaves(features->n_corners); |
||||
auto matches = orb_features.initMatches(features->n_corners); |
||||
|
||||
// copy out normalized keypoints
|
||||
for (int i = 0; i < features->n_corners; i++) { |
||||
xs.set(i, features->xy[i][0] * 1.0f / ORBD_WIDTH - 0.5f); |
||||
ys.set(i, features->xy[i][1] * 1.0f / ORBD_HEIGHT - 0.5f); |
||||
octaves.set(i, features->octave[i]); |
||||
matches.set(i, features->matches[i]); |
||||
match_count += features->matches[i] != -1; |
||||
} |
||||
|
||||
auto words = capnp::messageToFlatArray(msg); |
||||
auto bytes = words.asBytes(); |
||||
zmq_send(orb_features_sock, bytes.begin(), bytes.size(), 0); |
||||
} |
||||
|
||||
// *** send OrbFeaturesSummary ***
|
||||
|
||||
{ |
||||
// create capnp message
|
||||
capnp::MallocMessageBuilder msg; |
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
||||
event.setLogMonoTime(nanos_since_boot()); |
||||
|
||||
auto orb_features_summary = event.initOrbFeaturesSummary(); |
||||
|
||||
orb_features_summary.setTimestampEof(extra.timestamp_eof); |
||||
orb_features_summary.setTimestampLastEof(timestamp_last_eof); |
||||
orb_features_summary.setFeatureCount(features->n_corners); |
||||
orb_features_summary.setMatchCount(match_count); |
||||
orb_features_summary.setComputeNs(end-start); |
||||
|
||||
auto words = capnp::messageToFlatArray(msg); |
||||
auto bytes = words.asBytes(); |
||||
zmq_send(orb_features_summary_sock, bytes.begin(), bytes.size(), 0); |
||||
} |
||||
|
||||
timestamp_last_eof = extra.timestamp_eof; |
||||
} |
||||
} |
||||
visionstream_destroy(&stream); |
||||
return 0; |
||||
} |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:536c91fab8a36e7cdb06ade1599cad406a206c1b681bb1873fdb7c39e6213a58 |
||||
oid sha256:e694582549cfe8d1ed27d2e6e34b3f8dc1c01bf2a257c30251bd1af65229bf5d |
||||
size 981400 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:da501e3c27b8e736b6c99dac2b7c27ffb38aa43febe56b8134668250eeebb4aa |
||||
oid sha256:2cc5877fcc3d8ba9bc1ac50b4effcf62b3a865081d5c18b56528cfbef42eda4c |
||||
size 972296 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:120cc9f11d56e4b67a2b9600b04500601ad597f8f4516e1c22cf32330a891442 |
||||
size 13386048 |
||||
oid sha256:53701cb3f9469c23d4e48e8bb417c307f1b41a3de3172fe3031d85613d8b31c1 |
||||
size 13386064 |
||||
|
||||
Loading…
Reference in new issue