first commit
This commit is contained in:
leon
commit
5abc95def5
61
CMakeLists.txt
Normal file
61
CMakeLists.txt
Normal file
@ -0,0 +1,61 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
|
||||
PROJECT(lidar_camera)
|
||||
|
||||
set(CMAKE_SOURCE_DIR "./")
|
||||
|
||||
|
||||
message(STATUS "========================")
|
||||
message(STATUS ${CMAKE_SYSTEM_NAME})
|
||||
message(STATUS ${CMAKE_SYSTEM_PROCESSOR})
|
||||
message(STATUS "========================")
|
||||
|
||||
|
||||
find_package(OpenCV 3.2)
|
||||
if(NOT OpenCV_FOUND)
|
||||
message(FATAL_ERROR "OpenCV > 3.2 not found.")
|
||||
endif()
|
||||
MESSAGE(${OpenCV_VERSION})
|
||||
|
||||
# 检测操作系统和架构
|
||||
if(CMAKE_SYSTEM_NAME STREQUAL "Linux")
|
||||
if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64")
|
||||
set(PLATFORM "linux/x64")
|
||||
elseif(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
|
||||
set(PLATFORM "linux/aarch64")
|
||||
else()
|
||||
message(FATAL_ERROR "Unsupported architecture on Linux")
|
||||
endif()
|
||||
elseif(CMAKE_SYSTEM_NAME STREQUAL "Windows")
|
||||
if(CMAKE_SIZEOF_VOID_P EQUAL 8)
|
||||
set(PLATFORM "windows/x64")
|
||||
else()
|
||||
message(FATAL_ERROR "Unsupported architecture on Windows")
|
||||
endif()
|
||||
else()
|
||||
message(FATAL_ERROR "Unsupported operating system")
|
||||
endif()
|
||||
|
||||
# 输出当前系统和架构
|
||||
message(STATUS "operating system: ${PLATFORM}")
|
||||
|
||||
SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_SOURCE_DIR}/output/")
|
||||
|
||||
INCLUDE_DIRECTORIES(
|
||||
"/opt/HuarayTech/MVviewer/include"
|
||||
"/usr/include"
|
||||
)
|
||||
# LINK_DIRECTORIES(
|
||||
# "/opt/HuarayTech/MVviewer/lib"
|
||||
# "/opt/HuarayTech/MVviewer/bin"
|
||||
# )
|
||||
|
||||
aux_source_directory(${CMAKE_SOURCE_DIR}/ SRCLIST)
|
||||
|
||||
add_executable(${PROJECT_NAME} ${SRCLIST})
|
||||
|
||||
# target_link_libraries(${PROJECT_NAME} MVSDK)
|
||||
target_link_libraries(${PROJECT_NAME} dl)
|
||||
# target_link_libraries(${PROJECT_NAME} lrt)
|
||||
target_link_libraries(${PROJECT_NAME} pthread)
|
||||
target_link_libraries(${PROJECT_NAME} "/opt/HuarayTech/MVviewer/lib" ${OpenCV_LIBS})
|
||||
1086
CameraDriver.cpp
Normal file
1086
CameraDriver.cpp
Normal file
File diff suppressed because it is too large
Load Diff
99
CameraDriver.h
Normal file
99
CameraDriver.h
Normal file
@ -0,0 +1,99 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#ifndef _WIN32
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
#include <dlfcn.h>
|
||||
#endif // !_WIN32
|
||||
#include "IMVApi.h"
|
||||
#include "IMVDefines.h"
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
extern "C"{
|
||||
|
||||
#define MONO_CHANNEL_NUM 1
|
||||
#define RGB_CHANNEL_NUM 3
|
||||
#define BGR_CHANNEL_NUM 3
|
||||
|
||||
#define sleep(ms) usleep(1000 * ms)
|
||||
|
||||
typedef const char* (IMV_CALL * DLL_GetVersion) ();
|
||||
typedef int (IMV_CALL * DLL_EnumDevices) (OUT IMV_DeviceList *pDeviceList, IN unsigned int interfaceType);
|
||||
typedef int (IMV_CALL * DLL_EnumDevicesByUnicast) (OUT IMV_DeviceList *pDeviceList, IN const char* pIpAddress);
|
||||
typedef int (IMV_CALL * DLL_CreateHandle) (OUT IMV_HANDLE* handle, IN IMV_ECreateHandleMode mode, IN void* pIdentifier);
|
||||
typedef int (IMV_CALL * DLL_DestroyHandle) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_GetDeviceInfo) (IN IMV_HANDLE handle, OUT IMV_DeviceInfo *pDevInfo);
|
||||
typedef int (IMV_CALL * DLL_Open) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_OpenEx) (IN IMV_HANDLE handle, IN IMV_ECameraAccessPermission accessPermission);
|
||||
typedef bool (IMV_CALL * DLL_IsOpen) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_Close) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_GIGE_ForceIpAddress) (IN IMV_HANDLE handle, IN const char* pIpAddress, IN const char* pSubnetMask, IN const char* pGateway);
|
||||
typedef int (IMV_CALL * DLL_GIGE_GetAccessPermission) (IN IMV_HANDLE handle, IMV_ECameraAccessPermission* pAccessPermission);
|
||||
typedef int (IMV_CALL * DLL_GIGE_SetAnswerTimeout) (IN IMV_HANDLE handle, IN unsigned int timeout);
|
||||
typedef int (IMV_CALL * DLL_DownLoadGenICamXML) (IN IMV_HANDLE handle, IN const char* pFullFileName);
|
||||
typedef int (IMV_CALL * DLL_SaveDeviceCfg) (IN IMV_HANDLE handle, IN const char* pFullFileName);
|
||||
typedef int (IMV_CALL * DLL_LoadDeviceCfg) (IN IMV_HANDLE handle, IN const char* pFullFileName, OUT IMV_ErrorList* pErrorList);
|
||||
typedef int (IMV_CALL * DLL_WriteUserPrivateData) (IN IMV_HANDLE handle, IN void* pBuffer, IN_OUT unsigned int* pLength);
|
||||
typedef int (IMV_CALL * DLL_ReadUserPrivateData) (IN IMV_HANDLE handle, OUT void* pBuffer, IN_OUT unsigned int* pLength);
|
||||
typedef int (IMV_CALL * DLL_WriteUARTData) (IN IMV_HANDLE handle, IN void* pBuffer, IN_OUT unsigned int* pLength);
|
||||
typedef int (IMV_CALL * DLL_ReadUARTData) (IN IMV_HANDLE handle, OUT void* pBuffer, IN_OUT unsigned int* pLength);
|
||||
typedef int (IMV_CALL * DLL_SubscribeConnectArg) (IN IMV_HANDLE handle, IN IMV_ConnectCallBack proc, IN void* pUser);
|
||||
typedef int (IMV_CALL * DLL_SubscribeParamUpdateArg) (IN IMV_HANDLE handle, IN IMV_ParamUpdateCallBack proc, IN void* pUser);
|
||||
typedef int (IMV_CALL * DLL_SubscribeStreamArg) (IN IMV_HANDLE handle, IN IMV_StreamCallBack proc, IN void* pUser);
|
||||
typedef int (IMV_CALL * DLL_SubscribeMsgChannelArg) (IN IMV_HANDLE handle, IN IMV_MsgChannelCallBack proc, IN void* pUser);
|
||||
typedef int (IMV_CALL * DLL_SetBufferCount) (IN IMV_HANDLE handle, IN unsigned int nSize);
|
||||
typedef int (IMV_CALL * DLL_ClearFrameBuffer) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_GIGE_SetInterPacketTimeout) (IN IMV_HANDLE handle, IN unsigned int nTimeout);
|
||||
typedef int (IMV_CALL * DLL_GIGE_SetSingleResendMaxPacketNum) (IN IMV_HANDLE handle, IN unsigned int maxPacketNum);
|
||||
typedef int (IMV_CALL * DLL_GIGE_SetMaxLostPacketNum) (IN IMV_HANDLE handle, IN unsigned int maxLostPacketNum);
|
||||
typedef int (IMV_CALL * DLL_StartGrabbing) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_StartGrabbingEx) (IN IMV_HANDLE handle, IN uint64_t maxImagesGrabbed, IN IMV_EGrabStrategy strategy);
|
||||
typedef bool (IMV_CALL * DLL_IsGrabbing) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_StopGrabbing) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_AttachGrabbing) (IN IMV_HANDLE handle, IN IMV_FrameCallBack proc, IN void* pUser);
|
||||
typedef int (IMV_CALL * DLL_GetFrame) (IN IMV_HANDLE handle, OUT IMV_Frame* pFrame, IN unsigned int timeoutMS);
|
||||
typedef int (IMV_CALL * DLL_ReleaseFrame) (IN IMV_HANDLE handle, IN IMV_Frame* pFrame);
|
||||
typedef int (IMV_CALL * DLL_CloneFrame) (IN IMV_HANDLE handle, IN IMV_Frame* pFrame, OUT IMV_Frame* pCloneFrame);
|
||||
typedef int (IMV_CALL * DLL_GetChunkDataByIndex) (IN IMV_HANDLE handle, IN IMV_Frame* pFrame, IN unsigned int index, OUT IMV_ChunkDataInfo *pChunkDataInfo);
|
||||
typedef int (IMV_CALL * DLL_GetStatisticsInfo) (IN IMV_HANDLE handle, OUT IMV_StreamStatisticsInfo* pStreamStatsInfo);
|
||||
typedef int (IMV_CALL * DLL_ResetStatisticsInfo) (IN IMV_HANDLE handle);
|
||||
typedef bool (IMV_CALL * DLL_FeatureIsAvailable) (IN IMV_HANDLE handle, IN const char* pFeatureName);
|
||||
typedef bool (IMV_CALL * DLL_FeatureIsReadable) (IN IMV_HANDLE handle, IN const char* pFeatureName);
|
||||
typedef bool (IMV_CALL * DLL_FeatureIsWriteable) (IN IMV_HANDLE handle, IN const char* pFeatureName);
|
||||
typedef bool (IMV_CALL * DLL_FeatureIsStreamable) (IN IMV_HANDLE handle, IN const char* pFeatureName);
|
||||
typedef bool (IMV_CALL * DLL_FeatureIsValid) (IN IMV_HANDLE handle, IN const char* pFeatureName);
|
||||
typedef int (IMV_CALL * DLL_GetIntFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT int64_t* pIntValue);
|
||||
typedef int (IMV_CALL * DLL_GetIntFeatureMin) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT int64_t* pIntValue);
|
||||
typedef int (IMV_CALL * DLL_GetIntFeatureMax) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT int64_t* pIntValue);
|
||||
typedef int (IMV_CALL * DLL_GetIntFeatureInc) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT int64_t* pIntValue);
|
||||
typedef int (IMV_CALL * DLL_SetIntFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, IN int64_t intValue);
|
||||
typedef int (IMV_CALL * DLL_GetDoubleFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT double* pDoubleValue);
|
||||
typedef int (IMV_CALL * DLL_GetDoubleFeatureMin) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT double* pDoubleValue);
|
||||
typedef int (IMV_CALL * DLL_GetDoubleFeatureMax) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT double* pDoubleValue);
|
||||
typedef int (IMV_CALL * DLL_SetDoubleFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, IN double doubleValue);
|
||||
typedef int (IMV_CALL * DLL_GetBoolFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT bool* pBoolValue);
|
||||
typedef int (IMV_CALL * DLL_SetBoolFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, IN bool boolValue);
|
||||
typedef int (IMV_CALL * DLL_GetEnumFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT uint64_t* pEnumValue);
|
||||
typedef int (IMV_CALL * DLL_SetEnumFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, IN uint64_t enumValue);
|
||||
typedef int (IMV_CALL * DLL_GetEnumFeatureSymbol) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT IMV_String* pEnumSymbol);
|
||||
typedef int (IMV_CALL * DLL_SetEnumFeatureSymbol) (IN IMV_HANDLE handle, IN const char* pFeatureName, IN const char* pEnumSymbol);
|
||||
typedef int (IMV_CALL * DLL_GetEnumFeatureEntryNum) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT unsigned int* pEntryNum);
|
||||
typedef int (IMV_CALL * DLL_GetEnumFeatureEntrys) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT IMV_EnumEntryList* pEnumEntryList);
|
||||
typedef int (IMV_CALL * DLL_GetStringFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT IMV_String* pStringValue);
|
||||
typedef int (IMV_CALL * DLL_SetStringFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, IN const char* pStringValue);
|
||||
typedef int (IMV_CALL * DLL_ExecuteCommandFeature) (IN IMV_HANDLE handle, IN const char* pFeatureName);
|
||||
typedef int (IMV_CALL * DLL_PixelConvert) (IN IMV_HANDLE handle, IN_OUT IMV_PixelConvertParam* pstPixelConvertParam);
|
||||
typedef int (IMV_CALL * DLL_OpenRecord) (IN IMV_HANDLE handle, IN IMV_RecordParam *pstRecordParam);
|
||||
typedef int (IMV_CALL * DLL_InputOneFrame) (IN IMV_HANDLE handle, IN IMV_RecordFrameInfoParam *pstRecordFrameInfoParam);
|
||||
typedef int (IMV_CALL * DLL_CloseRecord) (IN IMV_HANDLE handle);
|
||||
|
||||
|
||||
int camera_init();
|
||||
int camera_start(int epx_time = 12 * 10000);
|
||||
int camera_stop();
|
||||
int camera_cap(void** data, int& w, int& h, int time_out);
|
||||
|
||||
}
|
||||
794
CameraPublisher.cpp
Normal file
794
CameraPublisher.cpp
Normal file
@ -0,0 +1,794 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
#include <dlfcn.h>
|
||||
#include "IMVApi.h"
|
||||
#include "IMVDefines.h"
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
extern "C"{
|
||||
|
||||
#define MONO_CHANNEL_NUM 1
|
||||
#define RGB_CHANNEL_NUM 3
|
||||
#define BGR_CHANNEL_NUM 3
|
||||
|
||||
#define sleep(ms) usleep(1000 * ms)
|
||||
|
||||
typedef const char* (IMV_CALL * DLL_GetVersion) ();
|
||||
typedef int (IMV_CALL * DLL_EnumDevices) (OUT IMV_DeviceList *pDeviceList, IN unsigned int interfaceType);
|
||||
typedef int (IMV_CALL * DLL_EnumDevicesByUnicast) (OUT IMV_DeviceList *pDeviceList, IN const char* pIpAddress);
|
||||
typedef int (IMV_CALL * DLL_CreateHandle) (OUT IMV_HANDLE* handle, IN IMV_ECreateHandleMode mode, IN void* pIdentifier);
|
||||
typedef int (IMV_CALL * DLL_DestroyHandle) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_GetDeviceInfo) (IN IMV_HANDLE handle, OUT IMV_DeviceInfo *pDevInfo);
|
||||
typedef int (IMV_CALL * DLL_Open) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_OpenEx) (IN IMV_HANDLE handle, IN IMV_ECameraAccessPermission accessPermission);
|
||||
typedef bool (IMV_CALL * DLL_IsOpen) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_Close) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_GIGE_ForceIpAddress) (IN IMV_HANDLE handle, IN const char* pIpAddress, IN const char* pSubnetMask, IN const char* pGateway);
|
||||
typedef int (IMV_CALL * DLL_GIGE_GetAccessPermission) (IN IMV_HANDLE handle, IMV_ECameraAccessPermission* pAccessPermission);
|
||||
typedef int (IMV_CALL * DLL_GIGE_SetAnswerTimeout) (IN IMV_HANDLE handle, IN unsigned int timeout);
|
||||
typedef int (IMV_CALL * DLL_DownLoadGenICamXML) (IN IMV_HANDLE handle, IN const char* pFullFileName);
|
||||
typedef int (IMV_CALL * DLL_SaveDeviceCfg) (IN IMV_HANDLE handle, IN const char* pFullFileName);
|
||||
typedef int (IMV_CALL * DLL_LoadDeviceCfg) (IN IMV_HANDLE handle, IN const char* pFullFileName, OUT IMV_ErrorList* pErrorList);
|
||||
typedef int (IMV_CALL * DLL_WriteUserPrivateData) (IN IMV_HANDLE handle, IN void* pBuffer, IN_OUT unsigned int* pLength);
|
||||
typedef int (IMV_CALL * DLL_ReadUserPrivateData) (IN IMV_HANDLE handle, OUT void* pBuffer, IN_OUT unsigned int* pLength);
|
||||
typedef int (IMV_CALL * DLL_WriteUARTData) (IN IMV_HANDLE handle, IN void* pBuffer, IN_OUT unsigned int* pLength);
|
||||
typedef int (IMV_CALL * DLL_ReadUARTData) (IN IMV_HANDLE handle, OUT void* pBuffer, IN_OUT unsigned int* pLength);
|
||||
typedef int (IMV_CALL * DLL_SubscribeConnectArg) (IN IMV_HANDLE handle, IN IMV_ConnectCallBack proc, IN void* pUser);
|
||||
typedef int (IMV_CALL * DLL_SubscribeParamUpdateArg) (IN IMV_HANDLE handle, IN IMV_ParamUpdateCallBack proc, IN void* pUser);
|
||||
typedef int (IMV_CALL * DLL_SubscribeStreamArg) (IN IMV_HANDLE handle, IN IMV_StreamCallBack proc, IN void* pUser);
|
||||
typedef int (IMV_CALL * DLL_SubscribeMsgChannelArg) (IN IMV_HANDLE handle, IN IMV_MsgChannelCallBack proc, IN void* pUser);
|
||||
typedef int (IMV_CALL * DLL_SetBufferCount) (IN IMV_HANDLE handle, IN unsigned int nSize);
|
||||
typedef int (IMV_CALL * DLL_ClearFrameBuffer) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_GIGE_SetInterPacketTimeout) (IN IMV_HANDLE handle, IN unsigned int nTimeout);
|
||||
typedef int (IMV_CALL * DLL_GIGE_SetSingleResendMaxPacketNum) (IN IMV_HANDLE handle, IN unsigned int maxPacketNum);
|
||||
typedef int (IMV_CALL * DLL_GIGE_SetMaxLostPacketNum) (IN IMV_HANDLE handle, IN unsigned int maxLostPacketNum);
|
||||
typedef int (IMV_CALL * DLL_StartGrabbing) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_StartGrabbingEx) (IN IMV_HANDLE handle, IN uint64_t maxImagesGrabbed, IN IMV_EGrabStrategy strategy);
|
||||
typedef bool (IMV_CALL * DLL_IsGrabbing) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_StopGrabbing) (IN IMV_HANDLE handle);
|
||||
typedef int (IMV_CALL * DLL_AttachGrabbing) (IN IMV_HANDLE handle, IN IMV_FrameCallBack proc, IN void* pUser);
|
||||
typedef int (IMV_CALL * DLL_GetFrame) (IN IMV_HANDLE handle, OUT IMV_Frame* pFrame, IN unsigned int timeoutMS);
|
||||
typedef int (IMV_CALL * DLL_ReleaseFrame) (IN IMV_HANDLE handle, IN IMV_Frame* pFrame);
|
||||
typedef int (IMV_CALL * DLL_CloneFrame) (IN IMV_HANDLE handle, IN IMV_Frame* pFrame, OUT IMV_Frame* pCloneFrame);
|
||||
typedef int (IMV_CALL * DLL_GetChunkDataByIndex) (IN IMV_HANDLE handle, IN IMV_Frame* pFrame, IN unsigned int index, OUT IMV_ChunkDataInfo *pChunkDataInfo);
|
||||
typedef int (IMV_CALL * DLL_GetStatisticsInfo) (IN IMV_HANDLE handle, OUT IMV_StreamStatisticsInfo* pStreamStatsInfo);
|
||||
typedef int (IMV_CALL * DLL_ResetStatisticsInfo) (IN IMV_HANDLE handle);
|
||||
typedef bool (IMV_CALL * DLL_FeatureIsAvailable) (IN IMV_HANDLE handle, IN const char* pFeatureName);
|
||||
typedef bool (IMV_CALL * DLL_FeatureIsReadable) (IN IMV_HANDLE handle, IN const char* pFeatureName);
|
||||
typedef bool (IMV_CALL * DLL_FeatureIsWriteable) (IN IMV_HANDLE handle, IN const char* pFeatureName);
|
||||
typedef bool (IMV_CALL * DLL_FeatureIsStreamable) (IN IMV_HANDLE handle, IN const char* pFeatureName);
|
||||
typedef bool (IMV_CALL * DLL_FeatureIsValid) (IN IMV_HANDLE handle, IN const char* pFeatureName);
|
||||
typedef int (IMV_CALL * DLL_GetIntFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT int64_t* pIntValue);
|
||||
typedef int (IMV_CALL * DLL_GetIntFeatureMin) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT int64_t* pIntValue);
|
||||
typedef int (IMV_CALL * DLL_GetIntFeatureMax) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT int64_t* pIntValue);
|
||||
typedef int (IMV_CALL * DLL_GetIntFeatureInc) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT int64_t* pIntValue);
|
||||
typedef int (IMV_CALL * DLL_SetIntFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, IN int64_t intValue);
|
||||
typedef int (IMV_CALL * DLL_GetDoubleFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT double* pDoubleValue);
|
||||
typedef int (IMV_CALL * DLL_GetDoubleFeatureMin) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT double* pDoubleValue);
|
||||
typedef int (IMV_CALL * DLL_GetDoubleFeatureMax) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT double* pDoubleValue);
|
||||
typedef int (IMV_CALL * DLL_SetDoubleFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, IN double doubleValue);
|
||||
typedef int (IMV_CALL * DLL_GetBoolFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT bool* pBoolValue);
|
||||
typedef int (IMV_CALL * DLL_SetBoolFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, IN bool boolValue);
|
||||
typedef int (IMV_CALL * DLL_GetEnumFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT uint64_t* pEnumValue);
|
||||
typedef int (IMV_CALL * DLL_SetEnumFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, IN uint64_t enumValue);
|
||||
typedef int (IMV_CALL * DLL_GetEnumFeatureSymbol) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT IMV_String* pEnumSymbol);
|
||||
typedef int (IMV_CALL * DLL_SetEnumFeatureSymbol) (IN IMV_HANDLE handle, IN const char* pFeatureName, IN const char* pEnumSymbol);
|
||||
typedef int (IMV_CALL * DLL_GetEnumFeatureEntryNum) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT unsigned int* pEntryNum);
|
||||
typedef int (IMV_CALL * DLL_GetEnumFeatureEntrys) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT IMV_EnumEntryList* pEnumEntryList);
|
||||
typedef int (IMV_CALL * DLL_GetStringFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, OUT IMV_String* pStringValue);
|
||||
typedef int (IMV_CALL * DLL_SetStringFeatureValue) (IN IMV_HANDLE handle, IN const char* pFeatureName, IN const char* pStringValue);
|
||||
typedef int (IMV_CALL * DLL_ExecuteCommandFeature) (IN IMV_HANDLE handle, IN const char* pFeatureName);
|
||||
typedef int (IMV_CALL * DLL_PixelConvert) (IN IMV_HANDLE handle, IN_OUT IMV_PixelConvertParam* pstPixelConvertParam);
|
||||
typedef int (IMV_CALL * DLL_OpenRecord) (IN IMV_HANDLE handle, IN IMV_RecordParam *pstRecordParam);
|
||||
typedef int (IMV_CALL * DLL_InputOneFrame) (IN IMV_HANDLE handle, IN IMV_RecordFrameInfoParam *pstRecordFrameInfoParam);
|
||||
typedef int (IMV_CALL * DLL_CloseRecord) (IN IMV_HANDLE handle);
|
||||
|
||||
|
||||
|
||||
// ***********开始： 这部分处理与SDK操作相机无关，用于显示设备列表 ***********
|
||||
// ***********BEGIN: These functions are not related to API call and used to display device info***********
|
||||
// 数据帧回调函数
|
||||
// Data frame callback function
|
||||
static void onGetFrame(IMV_Frame* pFrame, void* pUser)
|
||||
{
|
||||
if (pFrame == NULL)
|
||||
{
|
||||
printf("pFrame is NULL\n");
|
||||
return;
|
||||
}
|
||||
|
||||
printf("Get frame blockId = %llu\n", pFrame->frameInfo.blockId);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
static void displayDeviceInfo(IMV_DeviceList deviceInfoList)
|
||||
{
|
||||
IMV_DeviceInfo* pDevInfo = NULL;
|
||||
unsigned int cameraIndex = 0;
|
||||
char vendorNameCat[11];
|
||||
char cameraNameCat[16];
|
||||
|
||||
// 打印Title行
|
||||
// Print title line
|
||||
printf("\nIdx Type Vendor Model S/N DeviceUserID IP Address \n");
|
||||
printf("------------------------------------------------------------------------------\n");
|
||||
|
||||
for (cameraIndex = 0; cameraIndex < deviceInfoList.nDevNum; cameraIndex++)
|
||||
{
|
||||
pDevInfo = &deviceInfoList.pDevInfo[cameraIndex];
|
||||
// 设备列表的相机索引 最大表示字数：3
|
||||
// Camera index in device list, display in 3 characters
|
||||
printf("%-3d", cameraIndex + 1);
|
||||
|
||||
// 相机的设备类型（GigE，U3V，CL，PCIe）
|
||||
// Camera type
|
||||
switch (pDevInfo->nCameraType)
|
||||
{
|
||||
case typeGigeCamera:printf(" GigE");break;
|
||||
case typeU3vCamera:printf(" U3V ");break;
|
||||
case typeCLCamera:printf(" CL ");break;
|
||||
case typePCIeCamera:printf(" PCIe");break;
|
||||
default:printf(" ");break;
|
||||
}
|
||||
|
||||
// 制造商信息 最大表示字数：10
|
||||
// Camera vendor name, display in 10 characters
|
||||
if (strlen(pDevInfo->vendorName) > 10)
|
||||
{
|
||||
memcpy(vendorNameCat, pDevInfo->vendorName, 7);
|
||||
vendorNameCat[7] = '\0';
|
||||
strcat(vendorNameCat, "...");
|
||||
printf(" %-10.10s", vendorNameCat);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf(" %-10.10s", pDevInfo->vendorName);
|
||||
}
|
||||
|
||||
// 相机的型号信息 最大表示字数：10
|
||||
// Camera model name, display in 10 characters
|
||||
printf(" %-10.10s", pDevInfo->modelName);
|
||||
|
||||
// 相机的序列号 最大表示字数：15
|
||||
// Camera serial number, display in 15 characters
|
||||
printf(" %-15.15s", pDevInfo->serialNumber);
|
||||
|
||||
// 自定义用户ID 最大表示字数：15
|
||||
// Camera user id, display in 15 characters
|
||||
if (strlen(pDevInfo->cameraName) > 15)
|
||||
{
|
||||
memcpy(cameraNameCat, pDevInfo->cameraName, 12);
|
||||
cameraNameCat[12] = '\0';
|
||||
strcat(cameraNameCat, "...");
|
||||
printf(" %-15.15s", cameraNameCat);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf(" %-15.15s", pDevInfo->cameraName);
|
||||
}
|
||||
|
||||
// GigE相机时获取IP地址
|
||||
// IP address of GigE camera
|
||||
if (pDevInfo->nCameraType == typeGigeCamera)
|
||||
{
|
||||
printf(" %s", pDevInfo->DeviceSpecificInfo.gigeDeviceInfo.ipAddress);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
static char* trim(char* pStr)
|
||||
{
|
||||
char* pDst = pStr;
|
||||
char* pTemStr = NULL;
|
||||
|
||||
if (pDst != NULL)
|
||||
{
|
||||
pTemStr = pDst + strlen(pStr) - 1;
|
||||
while (*pDst == ' ')
|
||||
{
|
||||
pDst++;
|
||||
}
|
||||
while ((pTemStr > pDst) && (*pTemStr == ' '))
|
||||
{
|
||||
*pTemStr-- = '\0';
|
||||
}
|
||||
}
|
||||
return pDst;
|
||||
}
|
||||
|
||||
static int isInputValid(char* pInpuStr)
|
||||
{
|
||||
char numChar;
|
||||
char* pStr = pInpuStr;
|
||||
while (*pStr != '\0')
|
||||
{
|
||||
numChar = *pStr;
|
||||
if ((numChar > '9') || (numChar < '0'))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
pStr++;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static unsigned int selectDevice(unsigned int cameraCnt)
|
||||
{
|
||||
char inputStr[256];
|
||||
char* pTrimStr;
|
||||
int inputIndex = -1;
|
||||
int ret = -1;
|
||||
char* find = NULL;
|
||||
|
||||
printf("\nPlease input the camera index: ");
|
||||
while (1)
|
||||
{
|
||||
memset(inputStr, 0, sizeof(inputStr));
|
||||
fgets(inputStr, sizeof(inputStr), stdin);
|
||||
|
||||
// 清空输入缓存
|
||||
// clear flush
|
||||
fflush(stdin);
|
||||
|
||||
// fgets比gets多吃一个换行符号，取出换行符号
|
||||
// fgets eats one more line feed symbol than gets, and takes out the line feed symbol
|
||||
find = strchr(inputStr, '\n');
|
||||
if (find) { *find = '\0'; }
|
||||
|
||||
pTrimStr = trim(inputStr);
|
||||
ret = isInputValid(pTrimStr);
|
||||
if (ret == 0)
|
||||
{
|
||||
inputIndex = atoi(pTrimStr);
|
||||
// 输入的序号从1开始
|
||||
// Input index starts from 1
|
||||
inputIndex -= 1;
|
||||
if ((inputIndex >= 0) && (inputIndex < (int)cameraCnt))
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
printf("Input invalid! Please input the camera index: ");
|
||||
}
|
||||
return (unsigned int)inputIndex;
|
||||
}
|
||||
|
||||
// ***********结束： 这部分处理与SDK操作相机无关，用于显示设备列表 ***********
|
||||
// ***********END: These functions are not related to API call and used to display device info***********
|
||||
static int setSoftTriggerConf(void* libHandle, IMV_HANDLE devHandle)
|
||||
{
|
||||
int ret = IMV_OK;
|
||||
// 获取设置触发源为软触发函数地址
|
||||
DLL_SetEnumFeatureSymbol DLLSetEnumFeatureSymbol = (DLL_SetEnumFeatureSymbol)dlsym(libHandle, "IMV_SetEnumFeatureSymbol");
|
||||
if (NULL == DLLSetEnumFeatureSymbol)
|
||||
{
|
||||
printf("Get IMV_SetEnumFeatureSymbol address failed!\n");
|
||||
return ret;
|
||||
}
|
||||
|
||||
// 设置触发源为软触发
|
||||
// Set trigger source to Software
|
||||
ret = DLLSetEnumFeatureSymbol(devHandle, "TriggerSource", "Software");
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Set triggerSource value failed! ErrorCode[%d]\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// 设置触发器
|
||||
// Set trigger selector to FrameStart
|
||||
ret = DLLSetEnumFeatureSymbol(devHandle, "TriggerSelector", "FrameStart");
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Set triggerSelector value failed! ErrorCode[%d]\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// 设置触发模式
|
||||
// Set trigger mode to On
|
||||
ret = DLLSetEnumFeatureSymbol(devHandle, "TriggerMode", "On");
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Set triggerMode value failed! ErrorCode[%d]\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Image convert
|
||||
static int imageConvert(void* libHandle, IMV_HANDLE devHandle, IMV_Frame frame, IMV_EPixelType convertFormat)
|
||||
{
|
||||
IMV_PixelConvertParam stPixelConvertParam;
|
||||
unsigned char* pDstBuf = NULL;
|
||||
unsigned int nDstBufSize = 0;
|
||||
int ret = IMV_OK;
|
||||
FILE* hFile = NULL;
|
||||
const char* pFileName = NULL;
|
||||
const char* pConvertFormatStr = NULL;
|
||||
|
||||
// 获取设置触发源为软触发函数地址
|
||||
DLL_PixelConvert DLLPixelConvert = (DLL_PixelConvert)dlsym(libHandle, "IMV_PixelConvert");
|
||||
if (NULL == DLLPixelConvert)
|
||||
{
|
||||
printf("Get IMV_PixelConvert address failed!\n");
|
||||
return ret;
|
||||
}
|
||||
|
||||
switch (convertFormat)
|
||||
{
|
||||
case gvspPixelRGB8:
|
||||
nDstBufSize = sizeof(unsigned char) * frame.frameInfo.width * frame.frameInfo.height * 3;
|
||||
pFileName = (const char*)"convertRGB8.bmp";
|
||||
pConvertFormatStr = (const char*)"RGB8";
|
||||
break;
|
||||
|
||||
case gvspPixelBGR8:
|
||||
nDstBufSize = sizeof(unsigned char) * frame.frameInfo.width * frame.frameInfo.height * 3;
|
||||
pFileName = (const char*)"convertBGR8.bmp";
|
||||
pConvertFormatStr = (const char*)"BGR8";
|
||||
break;
|
||||
case gvspPixelBGRA8:
|
||||
nDstBufSize = sizeof(unsigned char) * frame.frameInfo.width * frame.frameInfo.height * 4;
|
||||
pFileName = (const char*)"convertBGRA8.bmp";
|
||||
pConvertFormatStr = (const char*)"BGRA8";
|
||||
break;
|
||||
case gvspPixelMono8:
|
||||
default:
|
||||
nDstBufSize = sizeof(unsigned char) * frame.frameInfo.width * frame.frameInfo.height;
|
||||
pFileName = (const char*)"convertMono8.bmp";
|
||||
pConvertFormatStr = (const char*)"Mono8";
|
||||
break;
|
||||
}
|
||||
|
||||
pDstBuf = (unsigned char*)malloc(nDstBufSize);
|
||||
if (NULL == pDstBuf)
|
||||
{
|
||||
printf("malloc pDstBuf failed!\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 图像转换成BGR8
|
||||
// convert image to BGR8
|
||||
memset(&stPixelConvertParam, 0, sizeof(stPixelConvertParam));
|
||||
stPixelConvertParam.nWidth = frame.frameInfo.width;
|
||||
stPixelConvertParam.nHeight = frame.frameInfo.height;
|
||||
stPixelConvertParam.ePixelFormat = frame.frameInfo.pixelFormat;
|
||||
stPixelConvertParam.pSrcData = frame.pData;
|
||||
stPixelConvertParam.nSrcDataLen = frame.frameInfo.size;
|
||||
stPixelConvertParam.nPaddingX = frame.frameInfo.paddingX;
|
||||
stPixelConvertParam.nPaddingY = frame.frameInfo.paddingY;
|
||||
stPixelConvertParam.eBayerDemosaic = demosaicNearestNeighbor;
|
||||
stPixelConvertParam.eDstPixelFormat = convertFormat;
|
||||
stPixelConvertParam.pDstBuf = pDstBuf;
|
||||
stPixelConvertParam.nDstBufSize = nDstBufSize;
|
||||
|
||||
ret = DLLPixelConvert(devHandle, &stPixelConvertParam);
|
||||
if (IMV_OK == ret)
|
||||
{
|
||||
printf("image convert to %s successfully! nDstDataLen (%u)\n",
|
||||
pConvertFormatStr, stPixelConvertParam.nDstBufSize);
|
||||
|
||||
// cv::Mat im(stPixelConvertParam.nHeight, stPixelConvertParam.nWidth, CV_8UC3, stPixelConvertParam.pDstBuf);
|
||||
// // 内参矩阵
|
||||
// cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 11057.154, 0, 4538.85, 0, 11044.943, 3350.918, 0, 0, 1);
|
||||
// // 设置畸变系数
|
||||
// cv::Mat distCoeffs = (cv::Mat_<double>(1, 5) << 0.311583980, -14.5864013, -0.00630134677, -0.00466401902, 183.662957);
|
||||
// // 准备输出图像
|
||||
// cv::Mat undistortedImg;
|
||||
// // 使用cv::undistort函数进行校正
|
||||
// cv::undistort(src, undistortedImg, cameraMatrix, distCoeffs);
|
||||
|
||||
cv::imwrite("/home/caowei/catkin_ws/output.png", undistortedImg);
|
||||
cv::imwrite("output.png", undistortedImg);
|
||||
|
||||
// hFile = fopen(pFileName, "wb");
|
||||
// if (hFile != NULL)
|
||||
// {
|
||||
// fwrite((void*)pDstBuf, 1, stPixelConvertParam.nDstBufSize, hFile);
|
||||
// fclose(hFile);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// // 如果打开失败，请用root权限执行
|
||||
// // If opefailed, Run as root
|
||||
// printf("Open file (%s) failed!\n", pFileName);
|
||||
// }
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("image convert to %s failed! ErrorCode[%d]\n", pConvertFormatStr, ret);
|
||||
}
|
||||
|
||||
if (pDstBuf)
|
||||
{
|
||||
free(pDstBuf);
|
||||
pDstBuf = NULL;
|
||||
}
|
||||
|
||||
return IMV_OK;
|
||||
}
|
||||
|
||||
static void sendToRos(IMV_Frame frame)
|
||||
{
|
||||
IMV_FlipImageParam stFlipImageParam;
|
||||
unsigned int nChannelNum = 0;
|
||||
int ret = IMV_OK;
|
||||
FILE* hFile = NULL;
|
||||
|
||||
memset(&stFlipImageParam, 0, sizeof(stFlipImageParam));
|
||||
|
||||
if (gvspPixelBGR8 == frame.frameInfo.pixelFormat)
|
||||
{
|
||||
stFlipImageParam.pSrcData = frame.pData;
|
||||
stFlipImageParam.nSrcDataLen = frame.frameInfo.width * frame.frameInfo.height * BGR_CHANNEL_NUM;
|
||||
stFlipImageParam.ePixelFormat = frame.frameInfo.pixelFormat;
|
||||
nChannelNum = BGR_CHANNEL_NUM;
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("image convert to BGR8 failed! ErrorCode[%d]\n", ret);
|
||||
}
|
||||
|
||||
// 向ros发送/image消息
|
||||
do
|
||||
{
|
||||
|
||||
} while (false);
|
||||
|
||||
}
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
int ret = IMV_OK;
|
||||
unsigned int cameraIndex = 0;
|
||||
IMV_HANDLE devHandle = NULL;
|
||||
void* libHandle = NULL;
|
||||
DLL_DestroyHandle DLLDestroyHandle = NULL;
|
||||
IMV_Frame frame;
|
||||
|
||||
// 加载SDK库
|
||||
// Load SDK library
|
||||
libHandle = dlopen("libMVSDK.so", RTLD_LAZY);
|
||||
if (NULL == libHandle)
|
||||
{
|
||||
printf("Load MVSDKmd.dll library failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 获取发现设备接口函数地址
|
||||
// Get discover camera interface address
|
||||
DLL_EnumDevices DLLEnumDevices = (DLL_EnumDevices)dlsym(libHandle, "IMV_EnumDevices");
|
||||
if (NULL == DLLEnumDevices)
|
||||
{
|
||||
printf("Get IMV_EnumDevices address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 获取创建设备句柄接口函数地址
|
||||
// Get create Device Handle interface address
|
||||
DLL_CreateHandle DLLCreateHandle = (DLL_CreateHandle)dlsym(libHandle, "IMV_CreateHandle");
|
||||
if (NULL == DLLCreateHandle)
|
||||
{
|
||||
printf("Get IMV_CreateHandle address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 获取销毁设备句柄接口函数地址
|
||||
// Get destroy Device Handle interface address
|
||||
DLLDestroyHandle = (DLL_DestroyHandle)dlsym(libHandle, "IMV_DestroyHandle");
|
||||
if (NULL == DLLDestroyHandle)
|
||||
{
|
||||
printf("Get IMV_DestroyHandle address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 获取打开相机接口函数地址
|
||||
// Get open camera interface address
|
||||
DLL_Open DLLOpen = (DLL_Open)dlsym(libHandle, "IMV_Open");
|
||||
if (NULL == DLLOpen)
|
||||
{
|
||||
printf("Get IMV_Open address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 获取注册数据帧回调接口函数地址
|
||||
// Get register data frame callback interface address
|
||||
DLL_AttachGrabbing DLLAttachGrabbing = (DLL_AttachGrabbing)dlsym(libHandle, "IMV_AttachGrabbing");
|
||||
if (NULL == DLLAttachGrabbing)
|
||||
{
|
||||
printf("Get IMV_AttachGrabbing address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 获取开始拉流接口函数地址
|
||||
// Get start grabbing interface address
|
||||
DLL_StartGrabbing DLLStartGrabbing = (DLL_StartGrabbing)dlsym(libHandle, "IMV_StartGrabbing");
|
||||
if (NULL == DLLStartGrabbing)
|
||||
{
|
||||
printf("Get IMV_StartGrabbing address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 获取停止拉流接口函数地址
|
||||
// Get stop grabbing interface address
|
||||
DLL_StopGrabbing DLLStopGrabbing = (DLL_StopGrabbing)dlsym(libHandle, "IMV_StopGrabbing");
|
||||
if (NULL == DLLStopGrabbing)
|
||||
{
|
||||
printf("Get IMV_StopGrabbing address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 获取
|
||||
|
||||
// 获取关闭相机接口函数地址
|
||||
// Get close camera interface address
|
||||
DLL_Close DLLClose = (DLL_Close)dlsym(libHandle, "IMV_Close");
|
||||
if (NULL == DLLClose)
|
||||
{
|
||||
printf("Get IMV_Close address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 获取获取一帧图像函数地址
|
||||
DLL_GetFrame DLLGetFrame = (DLL_GetFrame)dlsym(libHandle, "IMV_GetFrame");
|
||||
if (NULL == DLLGetFrame)
|
||||
{
|
||||
printf("Get IMV_GetFrame address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
DLL_ReleaseFrame DLLReleaseFrame = (DLL_ReleaseFrame)dlsym(libHandle, "IMV_ReleaseFrame");
|
||||
if (NULL == DLLReleaseFrame)
|
||||
{
|
||||
printf("Get IMV_ReleaseFrame address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
DLL_ClearFrameBuffer DLLClearFrameBuffer = (DLL_ClearFrameBuffer)dlsym(libHandle, "IMV_ClearFrameBuffer");
|
||||
if (NULL == DLLClearFrameBuffer)
|
||||
{
|
||||
printf("Get IMV_ClearFrameBuffer address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
DLL_ExecuteCommandFeature DLLExecuteCommandFeature = (DLL_ExecuteCommandFeature)dlsym(libHandle, "IMV_ExecuteCommandFeature");
|
||||
if (NULL == DLLExecuteCommandFeature)
|
||||
{
|
||||
printf("Get IMV_ExecuteCommandFeature address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
DLL_SetIntFeatureValue DLLSetIntFeatureValue = (DLL_SetIntFeatureValue)dlsym(libHandle, "IMV_SetIntFeatureValue");
|
||||
if (NULL == DLLSetIntFeatureValue)
|
||||
{
|
||||
printf("Get IMV_SetIntFeatureValue address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
DLL_SetDoubleFeatureValue DLLSetDoubleFeatureValue = (DLL_SetDoubleFeatureValue)dlsym(libHandle, "IMV_SetDoubleFeatureValue");
|
||||
if (NULL == DLLSetDoubleFeatureValue)
|
||||
{
|
||||
printf("Get IMV_SetDoubleFeatureValue address failed!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
////////////////////// 检查接口结束
|
||||
// 发现设备
|
||||
// discover camera
|
||||
IMV_DeviceList deviceInfoList;
|
||||
ret = DLLEnumDevices(&deviceInfoList, interfaceTypeAll);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Enumeration devices failed! ErrorCode[%d]\n", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (deviceInfoList.nDevNum < 1)
|
||||
{
|
||||
printf("no camera\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 打印相机基本信息（序号,类型,制造商信息,型号,序列号,用户自定义ID,IP地址）
|
||||
// Print camera info (Index, Type, Vendor, Model, Serial number, DeviceUserID, IP Address)
|
||||
|
||||
displayDeviceInfo(deviceInfoList);
|
||||
// 选择需要连接的相机
|
||||
// Select one camera to connect to
|
||||
// cameraIndex = selectDevice(deviceInfoList.nDevNum);
|
||||
cameraIndex = 0; // 第一个相机
|
||||
|
||||
// 创建设备句柄
|
||||
// Create Device Handle
|
||||
ret = DLLCreateHandle(&devHandle, modeByIndex, (void*)&cameraIndex);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Create devHandle failed! ErrorCode[%d]\n", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 打开相机
|
||||
ret = DLLOpen(devHandle);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Open camera failed! ErrorCode[%d]\n", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 设置软触发模式
|
||||
ret = setSoftTriggerConf(libHandle, devHandle);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("setSoftTriggerConf failed! ErrorCode[%d]\n", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
ret = DLLSetIntFeatureValue(devHandle, "Width", 9344);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Set feature value Width failed! ErrorCode[%d]\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = DLLSetIntFeatureValue(devHandle, "Height", 7000);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Set feature value Height failed! ErrorCode[%d]\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = DLLSetIntFeatureValue(devHandle, "OffsetX", 0);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Set feature value OffsetX failed! ErrorCode[%d]\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = DLLSetIntFeatureValue(devHandle, "OffsetY", 0);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Set feature value OffsetY failed! ErrorCode[%d]\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// 设置属性值曝光
|
||||
// Set feature value
|
||||
ret = DLLSetDoubleFeatureValue(devHandle, "ExposureTime", 12*10000);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Set feature value failed! ErrorCode[%d]\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// // 非多线程，不需要注册回调
|
||||
// // 注册数据帧回调函数
|
||||
// // Register data frame callback function
|
||||
// ret = DLLAttachGrabbing(devHandle, onGetFrame, NULL);
|
||||
// if (IMV_OK != ret)
|
||||
// {
|
||||
// printf("Attach grabbing failed! ErrorCode[%d]\n", ret);
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// 开始拉流
|
||||
// Start grabbing
|
||||
ret = DLLStartGrabbing(devHandle);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Start grabbing failed! ErrorCode[%d]\n", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/////////////////////////获取一帧图像////////////////////////////
|
||||
|
||||
// 清除帧数据缓存
|
||||
// Clear frame buffer
|
||||
ret = DLLClearFrameBuffer(devHandle);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Clear frame buffer failed! ErrorCode[%d]\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// 执行软触发
|
||||
// Execute soft trigger
|
||||
ret = DLLExecuteCommandFeature(devHandle, "TriggerSoftware");
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Execute TriggerSoftware failed! ErrorCode[%d]\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
// 获取一帧图像, TIMEOUT 5000ms
|
||||
ret = DLLGetFrame(devHandle, &frame, 5000);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Get frame failed! ErrorCode[%d]\n", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
printf("width %d\n", frame.frameInfo.width);
|
||||
printf("Height %d\n", frame.frameInfo.height);
|
||||
|
||||
ret = imageConvert(libHandle, devHandle, frame, gvspPixelBGR8);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("imageConvert failed! ErrorCode[%d]\n", ret);
|
||||
return 0;
|
||||
}
|
||||
// printf("11111111111111\n", ret);
|
||||
// cv::Mat im(frame.frameInfo.width, frame.frameInfo.height, CV_8UC3, frame.pData);
|
||||
// cv::imwrite("/home/caowei/catkin_ws/output.png", im);
|
||||
// cv::imwrite("output.png", im);
|
||||
// printf("22222222222222\n", ret);
|
||||
// // 向ros送 /image消息
|
||||
// sendToRos(frame);
|
||||
|
||||
// 释放图像缓存
|
||||
ret = DLLReleaseFrame(devHandle, &frame);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Release frame failed! ErrorCode[%d]\n", ret);
|
||||
return 0;
|
||||
}
|
||||
////////////////////////////////////////////////////////////////
|
||||
|
||||
// // 取图2秒
|
||||
// // get frame 2 seconds
|
||||
// sleep(2000);
|
||||
|
||||
// 停止拉流
|
||||
// Stop grabbing
|
||||
ret = DLLStopGrabbing(devHandle);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Stop grabbing failed! ErrorCode[%d]\n", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 关闭相机
|
||||
// Close camera
|
||||
ret = DLLClose(devHandle);
|
||||
if (IMV_OK != ret)
|
||||
{
|
||||
printf("Close camera failed! ErrorCode[%d]\n", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 销毁设备句柄
|
||||
// Destroy Device Handle
|
||||
if (NULL != devHandle)
|
||||
{
|
||||
// 销毁设备句柄
|
||||
// Destroy Device Handle
|
||||
DLLDestroyHandle(devHandle);
|
||||
}
|
||||
|
||||
if (NULL != libHandle)
|
||||
{
|
||||
dlclose(libHandle);
|
||||
}
|
||||
|
||||
printf("end...\n");
|
||||
// getchar();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
273
Corners.cpp
Normal file
273
Corners.cpp
Normal file
@ -0,0 +1,273 @@
|
||||
#include <cstdlib>
|
||||
#include <cstdio>
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
#include <fstream>
|
||||
#include <cmath>
|
||||
|
||||
#include "opencv2/opencv.hpp"
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/io/pcd_io.h>
|
||||
|
||||
#include <ros/package.h>
|
||||
|
||||
#include <pcl_ros/point_cloud.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <velodyne_pointcloud/point_types.h>
|
||||
#include <pcl/common/eigen.h>
|
||||
#include <pcl/common/transforms.h>
|
||||
#include <pcl/filters/passthrough.h>
|
||||
#include <pcl/sample_consensus/ransac.h>
|
||||
#include <pcl/sample_consensus/sac_model_line.h>
|
||||
#include <pcl/common/intersections.h>
|
||||
|
||||
|
||||
#include "lidar_camera_calibration/Utils.h"
|
||||
|
||||
int iteration_count = 0;
|
||||
std::vector< std::vector<cv::Point> > stored_corners;
|
||||
|
||||
bool getCorners(cv::Mat img, pcl::PointCloud<pcl::PointXYZ> scan, cv::Mat P, int num_of_markers, int MAX_ITERS)
|
||||
{
|
||||
|
||||
ROS_INFO_STREAM("iteration number: " << iteration_count << "\n");
|
||||
|
||||
/*Masking happens here */
|
||||
cv::Mat edge_mask = cv::Mat::zeros(img.size(), CV_8UC1);
|
||||
//edge_mask(cv::Rect(520, 205, 300, 250))=1;
|
||||
edge_mask(cv::Rect(0, 0, img.cols, img.rows))=1;
|
||||
img.copyTo(edge_mask, edge_mask);
|
||||
//pcl::io::savePCDFileASCII ("/home/vishnu/final1.pcd", scan.point_cloud);
|
||||
|
||||
img = edge_mask;
|
||||
|
||||
//cv:imwrite("/home/vishnu/marker.png", edge_mask);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ> pc = scan;
|
||||
//scan = Velodyne::Velodyne(filtered_pc);
|
||||
|
||||
cv::Rect frame(0, 0, img.cols, img.rows);
|
||||
|
||||
//pcl::io::savePCDFileASCII("/home/vishnu/final2.pcd", scan.point_cloud);
|
||||
|
||||
cv::Mat image_edge_laser = project(P, frame, scan, NULL);
|
||||
cv::threshold(image_edge_laser, image_edge_laser, 10, 255, 0);
|
||||
|
||||
cv::Mat combined_rgb_laser;
|
||||
std::vector<cv::Mat> rgb_laser_channels;
|
||||
|
||||
rgb_laser_channels.push_back(image_edge_laser);
|
||||
rgb_laser_channels.push_back(cv::Mat::zeros(image_edge_laser.size(), CV_8UC1));
|
||||
rgb_laser_channels.push_back(img);
|
||||
|
||||
cv::merge(rgb_laser_channels, combined_rgb_laser);
|
||||
/*cv::namedWindow("combined", cv::WINDOW_NORMAL);
|
||||
cv::imshow("combined", combined_rgb_laser);
|
||||
cv::waitKey(5);
|
||||
*/
|
||||
|
||||
std::map<std::pair<int, int>, std::vector<float> > c2D_to_3D;
|
||||
std::vector<float> point_3D;
|
||||
|
||||
/* store correspondences */
|
||||
for(pcl::PointCloud<pcl::PointXYZ>::iterator pt = pc.points.begin(); pt < pc.points.end(); pt++)
|
||||
{
|
||||
|
||||
// behind the camera
|
||||
if (pt->z < 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
cv::Point xy = project(*pt, P);
|
||||
if (xy.inside(frame))
|
||||
{
|
||||
//create a map of 2D and 3D points
|
||||
point_3D.clear();
|
||||
point_3D.push_back(pt->x);
|
||||
point_3D.push_back(pt->y);
|
||||
point_3D.push_back(pt->z);
|
||||
c2D_to_3D[std::pair<int, int>(xy.x, xy.y)] = point_3D;
|
||||
}
|
||||
}
|
||||
|
||||
/* print the correspondences */
|
||||
/*for(std::map<std::pair<int, int>, std::vector<float> >::iterator it=c2D_to_3D.begin(); it!=c2D_to_3D.end(); ++it)
|
||||
{
|
||||
std::cout << it->first.first << "," << it->first.second << " --> " << it->second[0] << "," <<it->second[1] << "," <<it->second[2] << "\n";
|
||||
}*/
|
||||
|
||||
/* get region of interest */
|
||||
|
||||
const int QUADS=num_of_markers;
|
||||
std::vector<int> LINE_SEGMENTS(QUADS, 4); //assuming each has 4 edges and 4 corners
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr board_corners(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr marker(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
std::vector<cv::Point3f> c_3D;
|
||||
std::vector<cv::Point2f> c_2D;
|
||||
|
||||
|
||||
cv::namedWindow("cloud", cv::WINDOW_NORMAL);
|
||||
cv::namedWindow("polygon", cv::WINDOW_NORMAL);
|
||||
//cv::namedWindow("combined", cv::WINDOW_NORMAL);
|
||||
|
||||
std::string pkg_loc = ros::package::getPath("lidar_camera_calibration");
|
||||
std::ofstream outfile(pkg_loc + "/conf/points.txt", std::ios_base::trunc);
|
||||
outfile << QUADS*4 << "\n";
|
||||
|
||||
for(int q=0; q<QUADS; q++)
|
||||
{
|
||||
std::cout << "---------Moving on to next marker--------\n";
|
||||
std::vector<Eigen::VectorXf> line_model;
|
||||
for(int i=0; i<LINE_SEGMENTS[q]; i++)
|
||||
{
|
||||
cv::Point _point_;
|
||||
std::vector<cv::Point> polygon;
|
||||
int collected;
|
||||
|
||||
// get markings in the first iteration only
|
||||
if(iteration_count == 0)
|
||||
{
|
||||
polygon.clear();
|
||||
collected = 0;
|
||||
// for test
|
||||
cv::setMouseCallback("cloud", onMouse, &_point_); //仅仅用于激活
|
||||
std::ifstream infile("/conf/self_define_point.txt");
|
||||
std::string line;
|
||||
|
||||
cv::imshow("cloud", image_edge_laser);
|
||||
cv::imwrite("cloud.png", image_edge_laser);
|
||||
cv::waitKey(0);
|
||||
while(collected != LINE_SEGMENTS[q])
|
||||
{
|
||||
|
||||
cv::setMouseCallback("cloud", onMouse, &_point_);
|
||||
std::getline(infile, line);
|
||||
_point_.x = atoi(line);
|
||||
std::getline(infile, line);
|
||||
_point_.y = atoi(line);
|
||||
|
||||
|
||||
++collected;
|
||||
//std::cout << _point_.x << " " << _point_.y << "\n";
|
||||
polygon.push_back(_point_);
|
||||
}
|
||||
stored_corners.push_back(polygon);
|
||||
}
|
||||
infile.close();
|
||||
polygon = stored_corners[4*q+i];
|
||||
|
||||
cv::Mat polygon_image = cv::Mat::zeros(image_edge_laser.size(), CV_8UC1);
|
||||
|
||||
rgb_laser_channels.clear();
|
||||
rgb_laser_channels.push_back(image_edge_laser);
|
||||
rgb_laser_channels.push_back(cv::Mat::zeros(image_edge_laser.size(), CV_8UC1));
|
||||
rgb_laser_channels.push_back(cv::Mat::zeros(image_edge_laser.size(), CV_8UC1));
|
||||
cv::merge(rgb_laser_channels, combined_rgb_laser);
|
||||
|
||||
for( int j = 0; j < 4; j++ )
|
||||
{
|
||||
cv::line(combined_rgb_laser, polygon[j], polygon[(j+1)%4], cv::Scalar(0, 255, 0));
|
||||
}
|
||||
|
||||
// initialize PointClouds
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
|
||||
|
||||
for(std::map<std::pair<int, int>, std::vector<float> >::iterator it=c2D_to_3D.begin(); it!=c2D_to_3D.end(); ++it)
|
||||
{
|
||||
|
||||
if (cv::pointPolygonTest(cv::Mat(polygon), cv::Point(it->first.first, it->first.second), true) > 0)
|
||||
{
|
||||
cloud->push_back(pcl::PointXYZ(it->second[0],it->second[1],it->second[2]));
|
||||
rectangle(combined_rgb_laser, cv::Point(it->first.first, it->first.second), cv::Point(it->first.first, it->first.second), cv::Scalar(0, 0, 255), 3, 8, 0); // RED point
|
||||
}
|
||||
}
|
||||
|
||||
if(cloud->size() < 2){ return false;}
|
||||
|
||||
cv::imshow("polygon", combined_rgb_laser);
|
||||
cv::waitKey(4);
|
||||
|
||||
//pcl::io::savePCDFileASCII("/home/vishnu/line_cloud.pcd", *cloud);
|
||||
|
||||
|
||||
|
||||
std::vector<int> inliers;
|
||||
Eigen::VectorXf model_coefficients;
|
||||
|
||||
|
||||
// created RandomSampleConsensus object and compute the appropriated model
|
||||
pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr model_l(new pcl::SampleConsensusModelLine<pcl::PointXYZ> (cloud));
|
||||
|
||||
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_l);
|
||||
// ransac.setDistanceThreshold (0.01);
|
||||
ransac.setDistanceThreshold (0.001);
|
||||
ransac.computeModel();
|
||||
ransac.getInliers(inliers);
|
||||
ransac.getModelCoefficients(model_coefficients);
|
||||
line_model.push_back(model_coefficients);
|
||||
|
||||
std::cout << "Line coefficients are:" << "\n" << model_coefficients << "\n";
|
||||
// copies all inliers of the model computed to another PointCloud
|
||||
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
|
||||
//pcl::io::savePCDFileASCII("/home/vishnu/RANSAC_line_cloud.pcd", *final);
|
||||
*marker += *final;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* calculate approximate intersection of lines */
|
||||
|
||||
|
||||
Eigen::Vector4f p1, p2, p_intersect;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr corners(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
for(int i=0; i<LINE_SEGMENTS[q]; i++)
|
||||
{
|
||||
pcl::lineToLineSegment(line_model[i], line_model[(i+1)%LINE_SEGMENTS[q]], p1, p2);
|
||||
for(int j=0; j<4; j++)
|
||||
{
|
||||
p_intersect(j) = (p1(j) + p2(j))/2.0;
|
||||
}
|
||||
c_3D.push_back(cv::Point3f(p_intersect(0), p_intersect(1), p_intersect(2)));
|
||||
corners->push_back(pcl::PointXYZ(p_intersect(0), p_intersect(1), p_intersect(2)));
|
||||
std::cout << "Point of intersection is approximately: \n" << p_intersect << "\n";
|
||||
//std::cout << "Distance between the lines: " << (p1 - p2).squaredNorm () << "\n";
|
||||
std::cout << p_intersect(0) << " " << p_intersect(1) << " " << p_intersect(2) << "\n";
|
||||
outfile << p_intersect(0) << " " << p_intersect(1) << " " << p_intersect(2) << "\n";
|
||||
|
||||
}
|
||||
|
||||
*board_corners += *corners;
|
||||
|
||||
std::cout << "Distance between the corners:\n";
|
||||
for(int i=0; i<4; i++)
|
||||
{
|
||||
std::cout <<
|
||||
|
||||
sqrt(
|
||||
pow(c_3D[4*q+i].x - c_3D[4*q+(i+1)%4].x, 2)
|
||||
+ pow(c_3D[4*q+i].y - c_3D[4*q+(i+1)%4].y, 2)
|
||||
+ pow(c_3D[4*q+i].z - c_3D[4*q+(i+1)%4].z, 2)
|
||||
)
|
||||
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
outfile.close();
|
||||
|
||||
iteration_count++;
|
||||
if(iteration_count == MAX_ITERS)
|
||||
{
|
||||
ros::shutdown();
|
||||
}
|
||||
return true;
|
||||
/* store point cloud with intersection points */
|
||||
//pcl::io::savePCDFileASCII("/home/vishnu/RANSAC_marker.pcd", *marker);
|
||||
//pcl::io::savePCDFileASCII("/home/vishnu/RANSAC_corners.pcd", *board_corners);
|
||||
}
|
||||
264
Corners.cpp.bak
Normal file
264
Corners.cpp.bak
Normal file
@ -0,0 +1,264 @@
|
||||
#include <cstdlib>
|
||||
#include <cstdio>
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
#include <fstream>
|
||||
#include <cmath>
|
||||
|
||||
#include "opencv2/opencv.hpp"
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/io/pcd_io.h>
|
||||
|
||||
#include <ros/package.h>
|
||||
|
||||
#include <pcl_ros/point_cloud.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <velodyne_pointcloud/point_types.h>
|
||||
#include <pcl/common/eigen.h>
|
||||
#include <pcl/common/transforms.h>
|
||||
#include <pcl/filters/passthrough.h>
|
||||
#include <pcl/sample_consensus/ransac.h>
|
||||
#include <pcl/sample_consensus/sac_model_line.h>
|
||||
#include <pcl/common/intersections.h>
|
||||
|
||||
|
||||
#include "lidar_camera_calibration/Utils.h"
|
||||
|
||||
int iteration_count = 0;
|
||||
std::vector< std::vector<cv::Point> > stored_corners;
|
||||
|
||||
bool getCorners(cv::Mat img, pcl::PointCloud<pcl::PointXYZ> scan, cv::Mat P, int num_of_markers, int MAX_ITERS)
|
||||
{
|
||||
|
||||
ROS_INFO_STREAM("iteration number: " << iteration_count << "\n");
|
||||
|
||||
/*Masking happens here */
|
||||
cv::Mat edge_mask = cv::Mat::zeros(img.size(), CV_8UC1);
|
||||
//edge_mask(cv::Rect(520, 205, 300, 250))=1;
|
||||
edge_mask(cv::Rect(0, 0, img.cols, img.rows))=1;
|
||||
img.copyTo(edge_mask, edge_mask);
|
||||
//pcl::io::savePCDFileASCII ("/home/vishnu/final1.pcd", scan.point_cloud);
|
||||
|
||||
img = edge_mask;
|
||||
|
||||
//cv:imwrite("/home/vishnu/marker.png", edge_mask);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ> pc = scan;
|
||||
//scan = Velodyne::Velodyne(filtered_pc);
|
||||
|
||||
cv::Rect frame(0, 0, img.cols, img.rows);
|
||||
|
||||
//pcl::io::savePCDFileASCII("/home/vishnu/final2.pcd", scan.point_cloud);
|
||||
|
||||
cv::Mat image_edge_laser = project(P, frame, scan, NULL);
|
||||
cv::threshold(image_edge_laser, image_edge_laser, 10, 255, 0);
|
||||
|
||||
|
||||
|
||||
|
||||
cv::Mat combined_rgb_laser;
|
||||
std::vector<cv::Mat> rgb_laser_channels;
|
||||
|
||||
rgb_laser_channels.push_back(image_edge_laser);
|
||||
rgb_laser_channels.push_back(cv::Mat::zeros(image_edge_laser.size(), CV_8UC1));
|
||||
rgb_laser_channels.push_back(img);
|
||||
|
||||
cv::merge(rgb_laser_channels, combined_rgb_laser);
|
||||
/*cv::namedWindow("combined", cv::WINDOW_NORMAL);
|
||||
cv::imshow("combined", combined_rgb_laser);
|
||||
cv::waitKey(5);
|
||||
*/
|
||||
|
||||
std::map<std::pair<int, int>, std::vector<float> > c2D_to_3D;
|
||||
std::vector<float> point_3D;
|
||||
|
||||
/* store correspondences */
|
||||
for(pcl::PointCloud<pcl::PointXYZ>::iterator pt = pc.points.begin(); pt < pc.points.end(); pt++)
|
||||
{
|
||||
|
||||
// behind the camera
|
||||
if (pt->z < 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
cv::Point xy = project(*pt, P);
|
||||
if (xy.inside(frame))
|
||||
{
|
||||
//create a map of 2D and 3D points
|
||||
point_3D.clear();
|
||||
point_3D.push_back(pt->x);
|
||||
point_3D.push_back(pt->y);
|
||||
point_3D.push_back(pt->z);
|
||||
c2D_to_3D[std::pair<int, int>(xy.x, xy.y)] = point_3D;
|
||||
}
|
||||
}
|
||||
|
||||
/* print the correspondences */
|
||||
/*for(std::map<std::pair<int, int>, std::vector<float> >::iterator it=c2D_to_3D.begin(); it!=c2D_to_3D.end(); ++it)
|
||||
{
|
||||
std::cout << it->first.first << "," << it->first.second << " --> " << it->second[0] << "," <<it->second[1] << "," <<it->second[2] << "\n";
|
||||
}*/
|
||||
|
||||
/* get region of interest */
|
||||
|
||||
const int QUADS=num_of_markers;
|
||||
std::vector<int> LINE_SEGMENTS(QUADS, 4); //assuming each has 4 edges and 4 corners
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr board_corners(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr marker(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
std::vector<cv::Point3f> c_3D;
|
||||
std::vector<cv::Point2f> c_2D;
|
||||
|
||||
|
||||
cv::namedWindow("cloud", cv::WINDOW_NORMAL);
|
||||
cv::namedWindow("polygon", cv::WINDOW_NORMAL);
|
||||
//cv::namedWindow("combined", cv::WINDOW_NORMAL);
|
||||
|
||||
std::string pkg_loc = ros::package::getPath("lidar_camera_calibration");
|
||||
std::ofstream outfile(pkg_loc + "/conf/points.txt", std::ios_base::trunc);
|
||||
outfile << QUADS*4 << "\n";
|
||||
|
||||
for(int q=0; q<QUADS; q++)
|
||||
{
|
||||
std::cout << "---------Moving on to next marker--------\n";
|
||||
std::vector<Eigen::VectorXf> line_model;
|
||||
for(int i=0; i<LINE_SEGMENTS[q]; i++)
|
||||
{
|
||||
cv::Point _point_;
|
||||
std::vector<cv::Point> polygon;
|
||||
int collected;
|
||||
|
||||
// get markings in the first iteration only
|
||||
if(iteration_count == 0)
|
||||
{
|
||||
polygon.clear();
|
||||
collected = 0;
|
||||
while(collected != LINE_SEGMENTS[q])
|
||||
{
|
||||
|
||||
cv::setMouseCallback("cloud", onMouse, &_point_);
|
||||
|
||||
cv::imshow("cloud", image_edge_laser);
|
||||
cv::waitKey(0);
|
||||
++collected;
|
||||
//std::cout << _point_.x << " " << _point_.y << "\n";
|
||||
polygon.push_back(_point_);
|
||||
}
|
||||
stored_corners.push_back(polygon);
|
||||
}
|
||||
|
||||
polygon = stored_corners[4*q+i];
|
||||
|
||||
cv::Mat polygon_image = cv::Mat::zeros(image_edge_laser.size(), CV_8UC1);
|
||||
|
||||
rgb_laser_channels.clear();
|
||||
rgb_laser_channels.push_back(image_edge_laser);
|
||||
rgb_laser_channels.push_back(cv::Mat::zeros(image_edge_laser.size(), CV_8UC1));
|
||||
rgb_laser_channels.push_back(cv::Mat::zeros(image_edge_laser.size(), CV_8UC1));
|
||||
cv::merge(rgb_laser_channels, combined_rgb_laser);
|
||||
|
||||
for( int j = 0; j < 4; j++ )
|
||||
{
|
||||
cv::line(combined_rgb_laser, polygon[j], polygon[(j+1)%4], cv::Scalar(0, 255, 0));
|
||||
}
|
||||
|
||||
// initialize PointClouds
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
|
||||
|
||||
for(std::map<std::pair<int, int>, std::vector<float> >::iterator it=c2D_to_3D.begin(); it!=c2D_to_3D.end(); ++it)
|
||||
{
|
||||
|
||||
if (cv::pointPolygonTest(cv::Mat(polygon), cv::Point(it->first.first, it->first.second), true) > 0)
|
||||
{
|
||||
cloud->push_back(pcl::PointXYZ(it->second[0],it->second[1],it->second[2]));
|
||||
rectangle(combined_rgb_laser, cv::Point(it->first.first, it->first.second), cv::Point(it->first.first, it->first.second), cv::Scalar(0, 0, 255), 3, 8, 0); // RED point
|
||||
}
|
||||
}
|
||||
|
||||
if(cloud->size() < 2){ return false;}
|
||||
|
||||
cv::imshow("polygon", combined_rgb_laser);
|
||||
cv::waitKey(4);
|
||||
|
||||
//pcl::io::savePCDFileASCII("/home/vishnu/line_cloud.pcd", *cloud);
|
||||
|
||||
|
||||
|
||||
std::vector<int> inliers;
|
||||
Eigen::VectorXf model_coefficients;
|
||||
|
||||
|
||||
// created RandomSampleConsensus object and compute the appropriated model
|
||||
pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr model_l(new pcl::SampleConsensusModelLine<pcl::PointXYZ> (cloud));
|
||||
|
||||
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_l);
|
||||
ransac.setDistanceThreshold (0.01);
|
||||
ransac.computeModel();
|
||||
ransac.getInliers(inliers);
|
||||
ransac.getModelCoefficients(model_coefficients);
|
||||
line_model.push_back(model_coefficients);
|
||||
|
||||
std::cout << "Line coefficients are:" << "\n" << model_coefficients << "\n";
|
||||
// copies all inliers of the model computed to another PointCloud
|
||||
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
|
||||
//pcl::io::savePCDFileASCII("/home/vishnu/RANSAC_line_cloud.pcd", *final);
|
||||
*marker += *final;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* calculate approximate intersection of lines */
|
||||
|
||||
|
||||
Eigen::Vector4f p1, p2, p_intersect;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr corners(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
for(int i=0; i<LINE_SEGMENTS[q]; i++)
|
||||
{
|
||||
pcl::lineToLineSegment(line_model[i], line_model[(i+1)%LINE_SEGMENTS[q]], p1, p2);
|
||||
for(int j=0; j<4; j++)
|
||||
{
|
||||
p_intersect(j) = (p1(j) + p2(j))/2.0;
|
||||
}
|
||||
c_3D.push_back(cv::Point3f(p_intersect(0), p_intersect(1), p_intersect(2)));
|
||||
corners->push_back(pcl::PointXYZ(p_intersect(0), p_intersect(1), p_intersect(2)));
|
||||
std::cout << "Point of intersection is approximately: \n" << p_intersect << "\n";
|
||||
//std::cout << "Distance between the lines: " << (p1 - p2).squaredNorm () << "\n";
|
||||
std::cout << p_intersect(0) << " " << p_intersect(1) << " " << p_intersect(2) << "\n";
|
||||
outfile << p_intersect(0) << " " << p_intersect(1) << " " << p_intersect(2) << "\n";
|
||||
|
||||
}
|
||||
|
||||
*board_corners += *corners;
|
||||
|
||||
std::cout << "Distance between the corners:\n";
|
||||
for(int i=0; i<4; i++)
|
||||
{
|
||||
std::cout <<
|
||||
|
||||
sqrt(
|
||||
pow(c_3D[4*q+i].x - c_3D[4*q+(i+1)%4].x, 2)
|
||||
+ pow(c_3D[4*q+i].y - c_3D[4*q+(i+1)%4].y, 2)
|
||||
+ pow(c_3D[4*q+i].z - c_3D[4*q+(i+1)%4].z, 2)
|
||||
)
|
||||
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
outfile.close();
|
||||
|
||||
iteration_count++;
|
||||
if(iteration_count == MAX_ITERS)
|
||||
{
|
||||
ros::shutdown();
|
||||
}
|
||||
return true;
|
||||
/* store point cloud with intersection points */
|
||||
//pcl::io::savePCDFileASCII("/home/vishnu/RANSAC_marker.pcd", *marker);
|
||||
//pcl::io::savePCDFileASCII("/home/vishnu/RANSAC_corners.pcd", *board_corners);
|
||||
}
|
||||
811
IMVDefines.h
Normal file
811
IMVDefines.h
Normal file
@ -0,0 +1,811 @@
|
||||
#ifndef __IMV_DEFINES_H__
|
||||
#define __IMV_DEFINES_H__
|
||||
|
||||
#ifdef WIN32
|
||||
typedef __int64 int64_t;
|
||||
typedef unsigned __int64 uint64_t;
|
||||
#else
|
||||
#include <stdint.h>
|
||||
#endif
|
||||
|
||||
#ifndef IN
|
||||
#define IN ///< \~chinese 输入型参数 \~english Input param
|
||||
#endif
|
||||
|
||||
#ifndef OUT
|
||||
#define OUT ///< \~chinese 输出型参数 \~english Output param
|
||||
#endif
|
||||
|
||||
#ifndef IN_OUT
|
||||
#define IN_OUT ///< \~chinese 输入/输出型参数 \~english Input/Output param
|
||||
#endif
|
||||
|
||||
#ifndef __cplusplus
|
||||
typedef char bool;
|
||||
#define true 1
|
||||
#define false 0
|
||||
#endif
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 错误码
|
||||
/// \~english
|
||||
/// \brief Error code
|
||||
#define IMV_OK 0 ///< \~chinese 成功，无错误 \~english Successed, no error
|
||||
#define IMV_ERROR -101 ///< \~chinese 通用的错误 \~english Generic error
|
||||
#define IMV_INVALID_HANDLE -102 ///< \~chinese 错误或无效的句柄 \~english Error or invalid handle
|
||||
#define IMV_INVALID_PARAM -103 ///< \~chinese 错误的参数 \~english Incorrect parameter
|
||||
#define IMV_INVALID_FRAME_HANDLE -104 ///< \~chinese 错误或无效的帧句柄 \~english Error or invalid frame handle
|
||||
#define IMV_INVALID_FRAME -105 ///< \~chinese 无效的帧 \~english Invalid frame
|
||||
#define IMV_INVALID_RESOURCE -106 ///< \~chinese 相机/事件/流等资源无效 \~english Camera/Event/Stream and so on resource invalid
|
||||
#define IMV_INVALID_IP -107 ///< \~chinese 设备与主机的IP网段不匹配 \~english Device's and PC's subnet is mismatch
|
||||
#define IMV_NO_MEMORY -108 ///< \~chinese 内存不足 \~english Malloc memery failed
|
||||
#define IMV_INSUFFICIENT_MEMORY -109 ///< \~chinese 传入的内存空间不足 \~english Insufficient memory
|
||||
#define IMV_ERROR_PROPERTY_TYPE -110 ///< \~chinese 属性类型错误 \~english Property type error
|
||||
#define IMV_INVALID_ACCESS -111 ///< \~chinese 属性不可访问、或不能读/写、或读/写失败 \~english Property not accessible, or not be read/written, or read/written failed
|
||||
#define IMV_INVALID_RANGE -112 ///< \~chinese 属性值超出范围、或者不是步长整数倍 \~english The property's value is out of range, or is not integer multiple of the step
|
||||
#define IMV_NOT_SUPPORT -113 ///< \~chinese 设备不支持的功能 \~english Device not supported function
|
||||
|
||||
#define IMV_MAX_DEVICE_ENUM_NUM 100 ///< \~chinese 支持设备最大个数 \~english The maximum number of supported devices
|
||||
#define IMV_MAX_STRING_LENTH 256 ///< \~chinese 字符串最大长度 \~english The maximum length of string
|
||||
#define IMV_MAX_ERROR_LIST_NUM 128 ///< \~chinese 失败属性列表最大长度 \~english The maximum size of failed properties list
|
||||
|
||||
typedef void* IMV_HANDLE; ///< \~chinese 设备句柄 \~english Device handle
|
||||
typedef void* IMV_FRAME_HANDLE; ///< \~chinese 帧句柄 \~english Frame handle
|
||||
|
||||
/// \~chinese
|
||||
///枚举：接口类型
|
||||
/// \~english
|
||||
///Enumeration: interface type
|
||||
typedef enum _IMV_EInterfaceType
|
||||
{
|
||||
interfaceTypeGige = 0x00000001, ///< \~chinese 网卡接口类型 \~english NIC type
|
||||
interfaceTypeUsb3 = 0x00000002, ///< \~chinese USB3.0接口类型 \~english USB3.0 interface type
|
||||
interfaceTypeCL = 0x00000004, ///< \~chinese CAMERALINK接口类型 \~english Cameralink interface type
|
||||
interfaceTypePCIe = 0x00000008, ///< \~chinese PCIe接口类型 \~english PCIe interface type
|
||||
interfaceTypeAll = 0x00000000, ///< \~chinese 忽略接口类型 \~english All types interface type
|
||||
interfaceInvalidType = 0xFFFFFFFF ///< \~chinese 无效接口类型 \~english Invalid interface type
|
||||
}IMV_EInterfaceType;
|
||||
|
||||
/// \~chinese
|
||||
///枚举：设备类型
|
||||
/// \~english
|
||||
///Enumeration: device type
|
||||
typedef enum _IMV_ECameraType
|
||||
{
|
||||
typeGigeCamera = 0, ///< \~chinese GIGE相机 \~english GigE Vision Camera
|
||||
typeU3vCamera = 1, ///< \~chinese USB3.0相机 \~english USB3.0 Vision Camera
|
||||
typeCLCamera = 2, ///< \~chinese CAMERALINK 相机 \~english Cameralink camera
|
||||
typePCIeCamera = 3, ///< \~chinese PCIe相机 \~english PCIe Camera
|
||||
typeUndefinedCamera = 255 ///< \~chinese 未知类型 \~english Undefined Camera
|
||||
}IMV_ECameraType;
|
||||
|
||||
/// \~chinese
|
||||
///枚举：创建句柄方式
|
||||
/// \~english
|
||||
///Enumeration: Create handle mode
|
||||
typedef enum _IMV_ECreateHandleMode
|
||||
{
|
||||
modeByIndex = 0, ///< \~chinese 通过已枚举设备的索引(从0开始，比如 0, 1, 2...) \~english By index of enumerated devices (Start from 0, such as 0, 1, 2...)
|
||||
modeByCameraKey, ///< \~chinese 通过设备键"厂商:序列号" \~english By device's key "vendor:serial number"
|
||||
modeByDeviceUserID, ///< \~chinese 通过设备自定义名 \~english By device userID
|
||||
modeByIPAddress, ///< \~chinese 通过设备IP地址 \~english By device IP address.
|
||||
}IMV_ECreateHandleMode;
|
||||
|
||||
/// \~chinese
|
||||
///枚举：访问权限
|
||||
/// \~english
|
||||
///Enumeration: access permission
|
||||
typedef enum _IMV_ECameraAccessPermission
|
||||
{
|
||||
accessPermissionOpen = 0, ///< \~chinese GigE相机没有被连接 \~english The GigE vision device isn't connected to any application.
|
||||
accessPermissionExclusive, ///< \~chinese 独占访问权限 \~english Exclusive Access Permission
|
||||
accessPermissionControl, ///< \~chinese 非独占可读访问权限 \~english Non-Exclusive Readbale Access Permission
|
||||
accessPermissionControlWithSwitchover, ///< \~chinese 切换控制访问权限 \~english Control access with switchover enabled.
|
||||
accessPermissionUnknown = 254, ///< \~chinese 无法确定 \~english Value not known; indeterminate.
|
||||
accessPermissionUndefined ///< \~chinese 未定义访问权限 \~english Undefined Access Permission
|
||||
}IMV_ECameraAccessPermission;
|
||||
|
||||
/// \~chinese
|
||||
///枚举：抓图策略
|
||||
/// \~english
|
||||
///Enumeration: grab strartegy
|
||||
typedef enum _IMV_EGrabStrategy
|
||||
{
|
||||
grabStrartegySequential = 0, ///< \~chinese 按到达顺序处理图片 \~english The images are processed in the order of their arrival
|
||||
grabStrartegyLatestImage = 1, ///< \~chinese 获取最新的图片 \~english Get latest image
|
||||
grabStrartegyUpcomingImage = 2, ///< \~chinese 等待获取下一张图片(只针对GigE相机) \~english Waiting for next image(GigE only)
|
||||
grabStrartegyUndefined ///< \~chinese 未定义 \~english Undefined
|
||||
}IMV_EGrabStrategy;
|
||||
|
||||
/// \~chinese
|
||||
///枚举：流事件状态
|
||||
/// \~english
|
||||
/// Enumeration:stream event status
|
||||
typedef enum _IMV_EEventStatus
|
||||
{
|
||||
streamEventNormal = 1, ///< \~chinese 正常流事件 \~english Normal stream event
|
||||
streamEventLostFrame = 2, ///< \~chinese 丢帧事件 \~english Lost frame event
|
||||
streamEventLostPacket = 3, ///< \~chinese 丢包事件 \~english Lost packet event
|
||||
streamEventImageError = 4, ///< \~chinese 图像错误事件 \~english Error image event
|
||||
streamEventStreamChannelError = 5, ///< \~chinese 取流错误事件 \~english Stream channel error event
|
||||
streamEventTooManyConsecutiveResends = 6, ///< \~chinese 太多连续重传 \~english Too many consecutive resends event
|
||||
streamEventTooManyLostPacket = 7 ///< \~chinese 太多丢包 \~english Too many lost packet event
|
||||
}IMV_EEventStatus;
|
||||
|
||||
/// \~chinese
|
||||
///枚举：图像转换Bayer格式所用的算法
|
||||
/// \~english
|
||||
/// Enumeration:alorithm used for Bayer demosaic
|
||||
typedef enum _IMV_EBayerDemosaic
|
||||
{
|
||||
demosaicNearestNeighbor, ///< \~chinese 最近邻 \~english Nearest neighbor
|
||||
demosaicBilinear, ///< \~chinese 双线性 \~english Bilinear
|
||||
demosaicEdgeSensing, ///< \~chinese 边缘检测 \~english Edge sensing
|
||||
demosaicNotSupport = 255, ///< \~chinese 不支持 \~english Not support
|
||||
}IMV_EBayerDemosaic;
|
||||
|
||||
/// \~chinese
|
||||
///枚举：事件类型
|
||||
/// \~english
|
||||
/// Enumeration:event type
|
||||
typedef enum _IMV_EVType
|
||||
{
|
||||
offLine, ///< \~chinese 设备离线通知 \~english device offline notification
|
||||
onLine ///< \~chinese 设备在线通知 \~english device online notification
|
||||
}IMV_EVType;
|
||||
|
||||
/// \~chinese
|
||||
///枚举：视频格式
|
||||
/// \~english
|
||||
/// Enumeration:Video format
|
||||
typedef enum _IMV_EVideoType
|
||||
{
|
||||
typeVideoFormatAVI = 0, ///< \~chinese AVI格式 \~english AVI format
|
||||
typeVideoFormatNotSupport = 255 ///< \~chinese 不支持 \~english Not support
|
||||
}IMV_EVideoType;
|
||||
|
||||
/// \~chinese
|
||||
///枚举：图像翻转类型
|
||||
/// \~english
|
||||
/// Enumeration:Image flip type
|
||||
typedef enum _IMV_EFlipType
|
||||
{
|
||||
typeFlipVertical, ///< \~chinese 垂直(Y轴)翻转 \~english Vertical(Y-axis) flip
|
||||
typeFlipHorizontal ///< \~chinese 水平(X轴)翻转 \~english Horizontal(X-axis) flip
|
||||
}IMV_EFlipType;
|
||||
|
||||
/// \~chinese
|
||||
///枚举：顺时针旋转角度
|
||||
/// \~english
|
||||
/// Enumeration:Rotation angle clockwise
|
||||
typedef enum _IMV_ERotationAngle
|
||||
{
|
||||
rotationAngle90, ///< \~chinese 顺时针旋转90度 \~english Rotate 90 degree clockwise
|
||||
rotationAngle180, ///< \~chinese 顺时针旋转180度 \~english Rotate 180 degree clockwise
|
||||
rotationAngle270, ///< \~chinese 顺时针旋转270度 \~english Rotate 270 degree clockwise
|
||||
}IMV_ERotationAngle;
|
||||
|
||||
#define IMV_GVSP_PIX_MONO 0x01000000
|
||||
#define IMV_GVSP_PIX_RGB 0x02000000
|
||||
#define IMV_GVSP_PIX_COLOR 0x02000000
|
||||
#define IMV_GVSP_PIX_CUSTOM 0x80000000
|
||||
#define IMV_GVSP_PIX_COLOR_MASK 0xFF000000
|
||||
|
||||
// Indicate effective number of bits occupied by the pixel (including padding).
|
||||
// This can be used to compute amount of memory required to store an image.
|
||||
#define IMV_GVSP_PIX_OCCUPY1BIT 0x00010000
|
||||
#define IMV_GVSP_PIX_OCCUPY2BIT 0x00020000
|
||||
#define IMV_GVSP_PIX_OCCUPY4BIT 0x00040000
|
||||
#define IMV_GVSP_PIX_OCCUPY8BIT 0x00080000
|
||||
#define IMV_GVSP_PIX_OCCUPY12BIT 0x000C0000
|
||||
#define IMV_GVSP_PIX_OCCUPY16BIT 0x00100000
|
||||
#define IMV_GVSP_PIX_OCCUPY24BIT 0x00180000
|
||||
#define IMV_GVSP_PIX_OCCUPY32BIT 0x00200000
|
||||
#define IMV_GVSP_PIX_OCCUPY36BIT 0x00240000
|
||||
#define IMV_GVSP_PIX_OCCUPY48BIT 0x00300000
|
||||
|
||||
/// \~chinese
|
||||
///枚举：图像格式
|
||||
/// \~english
|
||||
/// Enumeration:image format
|
||||
typedef enum _IMV_EPixelType
|
||||
{
|
||||
// Undefined pixel type
|
||||
gvspPixelTypeUndefined = -1,
|
||||
|
||||
// Mono Format
|
||||
gvspPixelMono1p = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY1BIT | 0x0037),
|
||||
gvspPixelMono2p = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY2BIT | 0x0038),
|
||||
gvspPixelMono4p = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY4BIT | 0x0039),
|
||||
gvspPixelMono8 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY8BIT | 0x0001),
|
||||
gvspPixelMono8S = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY8BIT | 0x0002),
|
||||
gvspPixelMono10 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x0003),
|
||||
gvspPixelMono10Packed = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY12BIT | 0x0004),
|
||||
gvspPixelMono12 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x0005),
|
||||
gvspPixelMono12Packed = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY12BIT | 0x0006),
|
||||
gvspPixelMono14 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x0025),
|
||||
gvspPixelMono16 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x0007),
|
||||
|
||||
// Bayer Format
|
||||
gvspPixelBayGR8 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY8BIT | 0x0008),
|
||||
gvspPixelBayRG8 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY8BIT | 0x0009),
|
||||
gvspPixelBayGB8 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY8BIT | 0x000A),
|
||||
gvspPixelBayBG8 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY8BIT | 0x000B),
|
||||
gvspPixelBayGR10 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x000C),
|
||||
gvspPixelBayRG10 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x000D),
|
||||
gvspPixelBayGB10 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x000E),
|
||||
gvspPixelBayBG10 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x000F),
|
||||
gvspPixelBayGR12 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x0010),
|
||||
gvspPixelBayRG12 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x0011),
|
||||
gvspPixelBayGB12 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x0012),
|
||||
gvspPixelBayBG12 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x0013),
|
||||
gvspPixelBayGR10Packed = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY12BIT | 0x0026),
|
||||
gvspPixelBayRG10Packed = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY12BIT | 0x0027),
|
||||
gvspPixelBayGB10Packed = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY12BIT | 0x0028),
|
||||
gvspPixelBayBG10Packed = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY12BIT | 0x0029),
|
||||
gvspPixelBayGR12Packed = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY12BIT | 0x002A),
|
||||
gvspPixelBayRG12Packed = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY12BIT | 0x002B),
|
||||
gvspPixelBayGB12Packed = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY12BIT | 0x002C),
|
||||
gvspPixelBayBG12Packed = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY12BIT | 0x002D),
|
||||
gvspPixelBayGR16 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x002E),
|
||||
gvspPixelBayRG16 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x002F),
|
||||
gvspPixelBayGB16 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x0030),
|
||||
gvspPixelBayBG16 = (IMV_GVSP_PIX_MONO | IMV_GVSP_PIX_OCCUPY16BIT | 0x0031),
|
||||
|
||||
// RGB Format
|
||||
gvspPixelRGB8 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY24BIT | 0x0014),
|
||||
gvspPixelBGR8 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY24BIT | 0x0015),
|
||||
gvspPixelRGBA8 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY32BIT | 0x0016),
|
||||
gvspPixelBGRA8 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY32BIT | 0x0017),
|
||||
gvspPixelRGB10 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY48BIT | 0x0018),
|
||||
gvspPixelBGR10 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY48BIT | 0x0019),
|
||||
gvspPixelRGB12 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY48BIT | 0x001A),
|
||||
gvspPixelBGR12 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY48BIT | 0x001B),
|
||||
gvspPixelRGB16 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY48BIT | 0x0033),
|
||||
gvspPixelRGB10V1Packed = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY32BIT | 0x001C),
|
||||
gvspPixelRGB10P32 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY32BIT | 0x001D),
|
||||
gvspPixelRGB12V1Packed = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY36BIT | 0X0034),
|
||||
gvspPixelRGB565P = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY16BIT | 0x0035),
|
||||
gvspPixelBGR565P = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY16BIT | 0X0036),
|
||||
|
||||
// YVR Format
|
||||
gvspPixelYUV411_8_UYYVYY = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY12BIT | 0x001E),
|
||||
gvspPixelYUV422_8_UYVY = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY16BIT | 0x001F),
|
||||
gvspPixelYUV422_8 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY16BIT | 0x0032),
|
||||
gvspPixelYUV8_UYV = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY24BIT | 0x0020),
|
||||
gvspPixelYCbCr8CbYCr = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY24BIT | 0x003A),
|
||||
gvspPixelYCbCr422_8 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY16BIT | 0x003B),
|
||||
gvspPixelYCbCr422_8_CbYCrY = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY16BIT | 0x0043),
|
||||
gvspPixelYCbCr411_8_CbYYCrYY = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY12BIT | 0x003C),
|
||||
gvspPixelYCbCr601_8_CbYCr = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY24BIT | 0x003D),
|
||||
gvspPixelYCbCr601_422_8 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY16BIT | 0x003E),
|
||||
gvspPixelYCbCr601_422_8_CbYCrY = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY16BIT | 0x0044),
|
||||
gvspPixelYCbCr601_411_8_CbYYCrYY = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY12BIT | 0x003F),
|
||||
gvspPixelYCbCr709_8_CbYCr = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY24BIT | 0x0040),
|
||||
gvspPixelYCbCr709_422_8 = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY16BIT | 0x0041),
|
||||
gvspPixelYCbCr709_422_8_CbYCrY = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY16BIT | 0x0045),
|
||||
gvspPixelYCbCr709_411_8_CbYYCrYY = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY12BIT | 0x0042),
|
||||
|
||||
// RGB Planar
|
||||
gvspPixelRGB8Planar = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY24BIT | 0x0021),
|
||||
gvspPixelRGB10Planar = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY48BIT | 0x0022),
|
||||
gvspPixelRGB12Planar = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY48BIT | 0x0023),
|
||||
gvspPixelRGB16Planar = (IMV_GVSP_PIX_COLOR | IMV_GVSP_PIX_OCCUPY48BIT | 0x0024),
|
||||
|
||||
//BayerRG10p和BayerRG12p格式，针对特定项目临时添加,请不要使用
|
||||
//BayerRG10p and BayerRG12p, currently used for specific project, please do not use them
|
||||
gvspPixelBayRG10p = 0x010A0058,
|
||||
gvspPixelBayRG12p = 0x010c0059,
|
||||
|
||||
//mono1c格式，自定义格式
|
||||
//mono1c, customized image format, used for binary output
|
||||
gvspPixelMono1c = 0x012000FF,
|
||||
|
||||
//mono1e格式，自定义格式，用来显示连通域
|
||||
//mono1e, customized image format, used for displaying connected domain
|
||||
gvspPixelMono1e = 0x01080FFF
|
||||
}IMV_EPixelType;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 字符串信息
|
||||
/// \~english
|
||||
/// \brief String information
|
||||
typedef struct _IMV_String
|
||||
{
|
||||
char str[IMV_MAX_STRING_LENTH]; ///< \~chinese 字符串.长度不超过256 \~english Strings and the maximum length of strings is 255.
|
||||
}IMV_String;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief GigE网卡信息
|
||||
/// \~english
|
||||
/// \brief GigE interface information
|
||||
typedef struct _IMV_GigEInterfaceInfo
|
||||
{
|
||||
char description[IMV_MAX_STRING_LENTH]; ///< \~chinese 网卡描述信息 \~english Network card description
|
||||
char macAddress[IMV_MAX_STRING_LENTH]; ///< \~chinese 网卡Mac地址 \~english Network card MAC Address
|
||||
char ipAddress[IMV_MAX_STRING_LENTH]; ///< \~chinese 设备Ip地址 \~english Device ip Address
|
||||
char subnetMask[IMV_MAX_STRING_LENTH]; ///< \~chinese 子网掩码 \~english SubnetMask
|
||||
char defaultGateWay[IMV_MAX_STRING_LENTH]; ///< \~chinese 默认网关 \~english Default GateWay
|
||||
char chReserved[5][IMV_MAX_STRING_LENTH]; ///< 保留 \~english Reserved field
|
||||
}IMV_GigEInterfaceInfo;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief USB接口信息
|
||||
/// \~english
|
||||
/// \brief USB interface information
|
||||
typedef struct _IMV_UsbInterfaceInfo
|
||||
{
|
||||
char description[IMV_MAX_STRING_LENTH]; ///< \~chinese USB接口描述信息 \~english USB interface description
|
||||
char vendorID[IMV_MAX_STRING_LENTH]; ///< \~chinese USB接口Vendor ID \~english USB interface Vendor ID
|
||||
char deviceID[IMV_MAX_STRING_LENTH]; ///< \~chinese USB接口设备ID \~english USB interface Device ID
|
||||
char subsystemID[IMV_MAX_STRING_LENTH]; ///< \~chinese USB接口Subsystem ID \~english USB interface Subsystem ID
|
||||
char revision[IMV_MAX_STRING_LENTH]; ///< \~chinese USB接口Revision \~english USB interface Revision
|
||||
char speed[IMV_MAX_STRING_LENTH]; ///< \~chinese USB接口speed \~english USB interface speed
|
||||
char chReserved[4][IMV_MAX_STRING_LENTH]; ///< 保留 \~english Reserved field
|
||||
}IMV_UsbInterfaceInfo;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief GigE设备信息
|
||||
/// \~english
|
||||
/// \brief GigE device information
|
||||
typedef struct _IMV_GigEDeviceInfo
|
||||
{
|
||||
/// \~chinese
|
||||
/// 设备支持的IP配置选项\n
|
||||
/// value:4 相机只支持LLA\n
|
||||
/// value:5 相机支持LLA和Persistent IP\n
|
||||
/// value:6 相机支持LLA和DHCP\n
|
||||
/// value:7 相机支持LLA、DHCP和Persistent IP\n
|
||||
/// value:0 获取失败
|
||||
/// \~english
|
||||
/// Supported IP configuration options of device\n
|
||||
/// value:4 Device supports LLA \n
|
||||
/// value:5 Device supports LLA and Persistent IP\n
|
||||
/// value:6 Device supports LLA and DHCP\n
|
||||
/// value:7 Device supports LLA, DHCP and Persistent IP\n
|
||||
/// value:0 Get fail
|
||||
unsigned int nIpConfigOptions;
|
||||
/// \~chinese
|
||||
/// 设备当前的IP配置选项\n
|
||||
/// value:4 LLA处于活动状态\n
|
||||
/// value:5 LLA和Persistent IP处于活动状态\n
|
||||
/// value:6 LLA和DHCP处于活动状态\n
|
||||
/// value:7 LLA、DHCP和Persistent IP处于活动状态\n
|
||||
/// value:0 获取失败
|
||||
/// \~english
|
||||
/// Current IP Configuration options of device\n
|
||||
/// value:4 LLA is active\n
|
||||
/// value:5 LLA and Persistent IP are active\n
|
||||
/// value:6 LLA and DHCP are active\n
|
||||
/// value:7 LLA, DHCP and Persistent IP are active\n
|
||||
/// value:0 Get fail
|
||||
unsigned int nIpConfigCurrent;
|
||||
unsigned int nReserved[3]; ///< \~chinese 保留 \~english Reserved field
|
||||
|
||||
char macAddress[IMV_MAX_STRING_LENTH]; ///< \~chinese 设备Mac地址 \~english Device MAC Address
|
||||
char ipAddress[IMV_MAX_STRING_LENTH]; ///< \~chinese 设备Ip地址 \~english Device ip Address
|
||||
char subnetMask[IMV_MAX_STRING_LENTH]; ///< \~chinese 子网掩码 \~english SubnetMask
|
||||
char defaultGateWay[IMV_MAX_STRING_LENTH]; ///< \~chinese 默认网关 \~english Default GateWay
|
||||
char protocolVersion[IMV_MAX_STRING_LENTH]; ///< \~chinese 网络协议版本 \~english Net protocol version
|
||||
/// \~chinese
|
||||
/// Ip配置有效性\n
|
||||
/// Ip配置有效时字符串值"Valid"\n
|
||||
/// Ip配置无效时字符串值"Invalid On This Interface"
|
||||
/// \~english
|
||||
/// IP configuration valid\n
|
||||
/// String value is "Valid" when ip configuration valid\n
|
||||
/// String value is "Invalid On This Interface" when ip configuration invalid
|
||||
char ipConfiguration[IMV_MAX_STRING_LENTH];
|
||||
char chReserved[6][IMV_MAX_STRING_LENTH]; ///< \~chinese 保留 \~english Reserved field
|
||||
|
||||
}IMV_GigEDeviceInfo;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief Usb设备信息
|
||||
/// \~english
|
||||
/// \brief Usb device information
|
||||
typedef struct _IMV_UsbDeviceInfo
|
||||
{
|
||||
bool bLowSpeedSupported; ///< \~chinese true支持，false不支持，其他值 非法。 \~english true support,false not supported,other invalid
|
||||
bool bFullSpeedSupported; ///< \~chinese true支持，false不支持，其他值 非法。 \~english true support,false not supported,other invalid
|
||||
bool bHighSpeedSupported; ///< \~chinese true支持，false不支持，其他值 非法。 \~english true support,false not supported,other invalid
|
||||
bool bSuperSpeedSupported; ///< \~chinese true支持，false不支持，其他值 非法。 \~english true support,false not supported,other invalid
|
||||
bool bDriverInstalled; ///< \~chinese true安装，false未安装，其他值 非法。 \~english true support,false not supported,other invalid
|
||||
bool boolReserved[3]; ///< \~chinese 保留
|
||||
unsigned int Reserved[4]; ///< \~chinese 保留 \~english Reserved field
|
||||
|
||||
char configurationValid[IMV_MAX_STRING_LENTH]; ///< \~chinese 配置有效性 \~english Configuration Valid
|
||||
char genCPVersion[IMV_MAX_STRING_LENTH]; ///< \~chinese GenCP 版本 \~english GenCP Version
|
||||
char u3vVersion[IMV_MAX_STRING_LENTH]; ///< \~chinese U3V 版本号 \~english U3v Version
|
||||
char deviceGUID[IMV_MAX_STRING_LENTH]; ///< \~chinese 设备引导号 \~english Device guid number
|
||||
char familyName[IMV_MAX_STRING_LENTH]; ///< \~chinese 设备系列号 \~english Device serial number
|
||||
char u3vSerialNumber[IMV_MAX_STRING_LENTH]; ///< \~chinese 设备序列号 \~english Device SerialNumber
|
||||
char speed[IMV_MAX_STRING_LENTH]; ///< \~chinese 设备传输速度 \~english Device transmission speed
|
||||
char maxPower[IMV_MAX_STRING_LENTH]; ///< \~chinese 设备最大供电量 \~english Maximum power supply of device
|
||||
char chReserved[4][IMV_MAX_STRING_LENTH]; ///< \~chinese 保留 \~english Reserved field
|
||||
|
||||
}IMV_UsbDeviceInfo;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 设备通用信息
|
||||
/// \~english
|
||||
/// \brief Device general information
|
||||
typedef struct _IMV_DeviceInfo
|
||||
{
|
||||
IMV_ECameraType nCameraType; ///< \~chinese 设备类别 \~english Camera type
|
||||
int nCameraReserved[5]; ///< \~chinese 保留 \~english Reserved field
|
||||
|
||||
char cameraKey[IMV_MAX_STRING_LENTH]; ///< \~chinese 厂商:序列号 \~english Camera key
|
||||
char cameraName[IMV_MAX_STRING_LENTH]; ///< \~chinese 用户自定义名 \~english UserDefinedName
|
||||
char serialNumber[IMV_MAX_STRING_LENTH]; ///< \~chinese 设备序列号 \~english Device SerialNumber
|
||||
char vendorName[IMV_MAX_STRING_LENTH]; ///< \~chinese 厂商 \~english Camera Vendor
|
||||
char modelName[IMV_MAX_STRING_LENTH]; ///< \~chinese 设备型号 \~english Device model
|
||||
char manufactureInfo[IMV_MAX_STRING_LENTH]; ///< \~chinese 设备制造信息 \~english Device ManufactureInfo
|
||||
char deviceVersion[IMV_MAX_STRING_LENTH]; ///< \~chinese 设备版本 \~english Device Version
|
||||
char cameraReserved[5][IMV_MAX_STRING_LENTH]; ///< \~chinese 保留 \~english Reserved field
|
||||
union
|
||||
{
|
||||
IMV_GigEDeviceInfo gigeDeviceInfo; ///< \~chinese Gige设备信息 \~english Gige Device Information
|
||||
IMV_UsbDeviceInfo usbDeviceInfo; ///< \~chinese Usb设备信息 \~english Usb Device Information
|
||||
}DeviceSpecificInfo;
|
||||
|
||||
IMV_EInterfaceType nInterfaceType; ///< \~chinese 接口类别 \~english Interface type
|
||||
int nInterfaceReserved[5]; ///< \~chinese 保留 \~english Reserved field
|
||||
char interfaceName[IMV_MAX_STRING_LENTH]; ///< \~chinese 接口名 \~english Interface Name
|
||||
char interfaceReserved[5][IMV_MAX_STRING_LENTH]; ///< \~chinese 保留 \~english Reserved field
|
||||
union
|
||||
{
|
||||
IMV_GigEInterfaceInfo gigeInterfaceInfo; ///< \~chinese GigE网卡信息 \~english Gige interface Information
|
||||
IMV_UsbInterfaceInfo usbInterfaceInfo; ///< \~chinese Usb接口信息 \~english Usb interface Information
|
||||
}InterfaceInfo;
|
||||
}IMV_DeviceInfo;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 加载失败的属性信息
|
||||
/// \~english
|
||||
/// \brief Load failed properties information
|
||||
typedef struct _IMV_ErrorList
|
||||
{
|
||||
unsigned int nParamCnt; ///< \~chinese 加载失败的属性个数 \~english The count of load failed properties
|
||||
IMV_String paramNameList[IMV_MAX_ERROR_LIST_NUM]; ///< \~chinese 加载失败的属性集合，上限128 \~english Array of load failed properties, up to 128
|
||||
}IMV_ErrorList;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 设备信息列表
|
||||
/// \~english
|
||||
/// \brief Device information list
|
||||
typedef struct _IMV_DeviceList
|
||||
{
|
||||
unsigned int nDevNum; ///< \~chinese 设备数量 \~english Device Number
|
||||
IMV_DeviceInfo* pDevInfo; ///< \~chinese 设备息列表(SDK内部缓存)，最多100设备 \~english Device information list(cached within the SDK), up to 100
|
||||
}IMV_DeviceList;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 连接事件信息
|
||||
/// \~english
|
||||
/// \brief connection event information
|
||||
typedef struct _IMV_SConnectArg
|
||||
{
|
||||
IMV_EVType event; ///< \~chinese 事件类型 \~english event type
|
||||
unsigned int nReserve[10]; ///< \~chinese 预留字段 \~english Reserved field
|
||||
}IMV_SConnectArg;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 参数更新事件信息
|
||||
/// \~english
|
||||
/// \brief Updating parameters event information
|
||||
typedef struct _IMV_SParamUpdateArg
|
||||
{
|
||||
bool isPoll; ///< \~chinese 是否是定时更新,true:表示是定时更新，false:表示非定时更新 \~english Update periodically or not. true:update periodically, true:not update periodically
|
||||
unsigned int nReserve[10]; ///< \~chinese 预留字段 \~english Reserved field
|
||||
unsigned int nParamCnt; ///< \~chinese 更新的参数个数 \~english The number of parameters which need update
|
||||
IMV_String* pParamNameList; ///< \~chinese 更新的参数名称集合(SDK内部缓存) \~english Array of parameter's name which need to be updated(cached within the SDK)
|
||||
}IMV_SParamUpdateArg;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 流事件信息
|
||||
/// \~english
|
||||
/// \brief Stream event information
|
||||
typedef struct _IMV_SStreamArg
|
||||
{
|
||||
unsigned int channel; ///< \~chinese 流通道号 \~english Channel no.
|
||||
uint64_t blockId; ///< \~chinese 流数据BlockID \~english Block ID of stream data
|
||||
uint64_t timeStamp; ///< \~chinese 时间戳 \~english Event time stamp
|
||||
IMV_EEventStatus eStreamEventStatus; ///< \~chinese 流事件状态码 \~english Stream event status code
|
||||
unsigned int status; ///< \~chinese 事件状态错误码 \~english Status error code
|
||||
unsigned int nReserve[9]; ///< \~chinese 预留字段 \~english Reserved field
|
||||
}IMV_SStreamArg;
|
||||
|
||||
/// \~chinese
|
||||
/// 消息通道事件ID列表
|
||||
/// \~english
|
||||
/// message channel event id list
|
||||
#define IMV_MSG_EVENT_ID_EXPOSURE_END 0x9001
|
||||
#define IMV_MSG_EVENT_ID_FRAME_TRIGGER 0x9002
|
||||
#define IMV_MSG_EVENT_ID_FRAME_START 0x9003
|
||||
#define IMV_MSG_EVENT_ID_ACQ_START 0x9004
|
||||
#define IMV_MSG_EVENT_ID_ACQ_TRIGGER 0x9005
|
||||
#define IMV_MSG_EVENT_ID_DATA_READ_OUT 0x9006
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 消息通道事件信息
|
||||
/// \~english
|
||||
/// \brief Message channel event information
|
||||
typedef struct _IMV_SMsgChannelArg
|
||||
{
|
||||
unsigned short eventId; ///< \~chinese 事件Id \~english Event id
|
||||
unsigned short channelId; ///< \~chinese 消息通道号 \~english Channel id
|
||||
uint64_t blockId; ///< \~chinese 流数据BlockID \~english Block ID of stream data
|
||||
uint64_t timeStamp; ///< \~chinese 时间戳 \~english Event timestamp
|
||||
unsigned int nReserve[8]; ///< \~chinese 预留字段 \~english Reserved field
|
||||
unsigned int nParamCnt; ///< \~chinese 参数个数 \~english The number of parameters which need update
|
||||
IMV_String* pParamNameList; ///< \~chinese 事件相关的属性名列集合(SDK内部缓存) \~english Array of parameter's name which is related(cached within the SDK)
|
||||
}IMV_SMsgChannelArg;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief Chunk数据信息
|
||||
/// \~english
|
||||
/// \brief Chunk data information
|
||||
typedef struct _IMV_ChunkDataInfo
|
||||
{
|
||||
unsigned int chunkID; ///< \~chinese ChunkID \~english ChunkID
|
||||
unsigned int nParamCnt; ///< \~chinese 属性名个数 \~english The number of paramNames
|
||||
IMV_String* pParamNameList; ///< \~chinese Chunk数据对应的属性名集合(SDK内部缓存) \~english ParamNames Corresponding property name of chunk data(cached within the SDK)
|
||||
}IMV_ChunkDataInfo;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 帧图像信息
|
||||
/// \~english
|
||||
/// \brief The frame image information
|
||||
typedef struct _IMV_FrameInfo
|
||||
{
|
||||
uint64_t blockId; ///< \~chinese 帧Id(仅对GigE/Usb/PCIe相机有效) \~english The block ID(GigE/Usb/PCIe camera only)
|
||||
unsigned int status; ///< \~chinese 数据帧状态(0是正常状态) \~english The status of frame(0 is normal status)
|
||||
unsigned int width; ///< \~chinese 图像宽度 \~english The width of image
|
||||
unsigned int height; ///< \~chinese 图像高度 \~english The height of image
|
||||
unsigned int size; ///< \~chinese 图像大小 \~english The size of image
|
||||
IMV_EPixelType pixelFormat; ///< \~chinese 图像像素格式 \~english The pixel format of image
|
||||
uint64_t timeStamp; ///< \~chinese 图像时间戳(仅对GigE/Usb/PCIe相机有效) \~english The timestamp of image(GigE/Usb/PCIe camera only)
|
||||
unsigned int chunkCount; ///< \~chinese 帧数据中包含的Chunk个数(仅对GigE/Usb相机有效) \~english The number of chunk in frame data(GigE/Usb Camera Only)
|
||||
unsigned int paddingX; ///< \~chinese 图像paddingX(仅对GigE/Usb/PCIe相机有效) \~english The paddingX of image(GigE/Usb/PCIe camera only)
|
||||
unsigned int paddingY; ///< \~chinese 图像paddingY(仅对GigE/Usb/PCIe相机有效) \~english The paddingY of image(GigE/Usb/PCIe camera only)
|
||||
unsigned int recvFrameTime; ///< \~chinese 图像在网络传输所用的时间(单位:微秒,非GigE相机该值为0) \~english The time taken for the image to be transmitted over the network(unit:us, The value is 0 for non-GigE camera)
|
||||
unsigned int nReserved[19]; ///< \~chinese 预留字段 \~english Reserved field
|
||||
}IMV_FrameInfo;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 帧图像数据信息
|
||||
/// \~english
|
||||
/// \brief Frame image data information
|
||||
typedef struct _IMV_Frame
|
||||
{
|
||||
IMV_FRAME_HANDLE frameHandle; ///< \~chinese 帧图像句柄(SDK内部帧管理用) \~english Frame image handle(used for managing frame within the SDK)
|
||||
unsigned char* pData; ///< \~chinese 帧图像数据的内存首地址 \~english The starting address of memory of image data
|
||||
IMV_FrameInfo frameInfo; ///< \~chinese 帧信息 \~english Frame information
|
||||
unsigned int nReserved[10]; ///< \~chinese 预留字段 \~english Reserved field
|
||||
}IMV_Frame;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief PCIE设备统计流信息
|
||||
/// \~english
|
||||
/// \brief PCIE device stream statistics information
|
||||
typedef struct _IMV_PCIEStreamStatsInfo
|
||||
{
|
||||
unsigned int imageError; ///< \~chinese 图像错误的帧数 \~english Number of images error frames
|
||||
unsigned int lostPacketBlock; ///< \~chinese 丢包的帧数 \~english Number of frames lost
|
||||
unsigned int nReserved0[10]; ///< \~chinese 预留 \~english Reserved field
|
||||
|
||||
unsigned int imageReceived; ///< \~chinese 正常获取的帧数 \~english Number of frames acquired
|
||||
double fps; ///< \~chinese 帧率 \~english Frame rate
|
||||
double bandwidth; ///< \~chinese 带宽(Mbps) \~english Bandwidth(Mbps)
|
||||
unsigned int nReserved[8]; ///< \~chinese 预留 \~english Reserved field
|
||||
}IMV_PCIEStreamStatsInfo;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief U3V设备统计流信息
|
||||
/// \~english
|
||||
/// \brief U3V device stream statistics information
|
||||
typedef struct _IMV_U3VStreamStatsInfo
|
||||
{
|
||||
unsigned int imageError; ///< \~chinese 图像错误的帧数 \~english Number of images error frames
|
||||
unsigned int lostPacketBlock; ///< \~chinese 丢包的帧数 \~english Number of frames lost
|
||||
unsigned int nReserved0[10]; ///< \~chinese 预留 \~english Reserved field
|
||||
|
||||
unsigned int imageReceived; ///< \~chinese 正常获取的帧数 \~english Number of images error frames
|
||||
double fps; ///< \~chinese 帧率 \~english Frame rate
|
||||
double bandwidth; ///< \~chinese 带宽(Mbps) \~english Bandwidth(Mbps)
|
||||
unsigned int nReserved[8]; ///< \~chinese 预留 \~english Reserved field
|
||||
}IMV_U3VStreamStatsInfo;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief Gige设备统计流信息
|
||||
/// \~english
|
||||
/// \brief Gige device stream statistics information
|
||||
typedef struct _IMV_GigEStreamStatsInfo
|
||||
{
|
||||
unsigned int nReserved0[10]; ///< \~chinese 预留 \~english Reserved field
|
||||
|
||||
unsigned int imageError; ///< \~chinese 图像错误的帧数 \~english Number of image error frames
|
||||
unsigned int lostPacketBlock; ///< \~chinese 丢包的帧数 \~english Number of frames lost
|
||||
unsigned int nReserved1[4]; ///< \~chinese 预留 \~english Reserved field
|
||||
unsigned int nReserved2[5]; ///< \~chinese 预留 \~english Reserved field
|
||||
|
||||
unsigned int imageReceived; ///< \~chinese 正常获取的帧数 \~english Number of frames acquired
|
||||
double fps; ///< \~chinese 帧率 \~english Frame rate
|
||||
double bandwidth; ///< \~chinese 带宽(Mbps) \~english Bandwidth(Mbps)
|
||||
unsigned int nReserved[4]; ///< \~chinese 预留 \~english Reserved field
|
||||
}IMV_GigEStreamStatsInfo;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 统计流信息
|
||||
/// \~english
|
||||
/// \brief Stream statistics information
|
||||
typedef struct _IMV_StreamStatisticsInfo
|
||||
{
|
||||
IMV_ECameraType nCameraType; ///< \~chinese 设备类型 \~english Device type
|
||||
|
||||
union
|
||||
{
|
||||
IMV_PCIEStreamStatsInfo pcieStatisticsInfo; ///< \~chinese PCIE设备统计信息 \~english PCIE device statistics information
|
||||
IMV_U3VStreamStatsInfo u3vStatisticsInfo; ///< \~chinese U3V设备统计信息 \~english U3V device statistics information
|
||||
IMV_GigEStreamStatsInfo gigeStatisticsInfo; ///< \~chinese Gige设备统计信息 \~english GIGE device statistics information
|
||||
};
|
||||
}IMV_StreamStatisticsInfo;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 枚举属性的枚举值信息
|
||||
/// \~english
|
||||
/// \brief Enumeration property 's enumeration value information
|
||||
typedef struct _IMV_EnumEntryInfo
|
||||
{
|
||||
uint64_t value; ///< \~chinese 枚举值 \~english Enumeration value
|
||||
char name[IMV_MAX_STRING_LENTH]; ///< \~chinese symbol名 \~english Symbol name
|
||||
}IMV_EnumEntryInfo;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 枚举属性的可设枚举值列表信息
|
||||
/// \~english
|
||||
/// \brief Enumeration property 's settable enumeration value list information
|
||||
typedef struct _IMV_EnumEntryList
|
||||
{
|
||||
unsigned int nEnumEntryBufferSize; ///< \~chinese 存放枚举值内存大小 \~english The size of saving enumeration value
|
||||
IMV_EnumEntryInfo* pEnumEntryInfo; ///< \~chinese 存放可设枚举值列表(调用者分配缓存) \~english Save the list of settable enumeration value(allocated cache by the caller)
|
||||
}IMV_EnumEntryList;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 像素转换结构体
|
||||
/// \~english
|
||||
/// \brief Pixel convert structure
|
||||
typedef struct _IMV_PixelConvertParam
|
||||
{
|
||||
unsigned int nWidth; ///< [IN] \~chinese 图像宽 \~english Width
|
||||
unsigned int nHeight; ///< [IN] \~chinese 图像高 \~english Height
|
||||
IMV_EPixelType ePixelFormat; ///< [IN] \~chinese 像素格式 \~english Pixel format
|
||||
unsigned char* pSrcData; ///< [IN] \~chinese 输入图像数据 \~english Input image data
|
||||
unsigned int nSrcDataLen; ///< [IN] \~chinese 输入图像长度 \~english Input image length
|
||||
unsigned int nPaddingX; ///< [IN] \~chinese 图像宽填充 \~english Padding X
|
||||
unsigned int nPaddingY; ///< [IN] \~chinese 图像高填充 \~english Padding Y
|
||||
IMV_EBayerDemosaic eBayerDemosaic; ///< [IN] \~chinese 转换Bayer格式算法 \~english Alorithm used for Bayer demosaic
|
||||
IMV_EPixelType eDstPixelFormat; ///< [IN] \~chinese 目标像素格式 \~english Destination pixel format
|
||||
unsigned char* pDstBuf; ///< [OUT] \~chinese 输出数据缓存(调用者分配缓存) \~english Output data buffer(allocated cache by the caller)
|
||||
unsigned int nDstBufSize; ///< [IN] \~chinese 提供的输出缓冲区大小 \~english Provided output buffer size
|
||||
unsigned int nDstDataLen; ///< [OUT] \~chinese 输出数据长度 \~english Output data length
|
||||
unsigned int nReserved[8]; ///< \~chinese 预留 \~english Reserved field
|
||||
}IMV_PixelConvertParam;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 录像结构体
|
||||
/// \~english
|
||||
/// \brief Record structure
|
||||
typedef struct _IMV_RecordParam
|
||||
{
|
||||
unsigned int nWidth; ///< [IN] \~chinese 图像宽 \~english Width
|
||||
unsigned int nHeight; ///< [IN] \~chinese 图像高 \~english Height
|
||||
float fFameRate; ///< [IN] \~chinese 帧率(大于0) \~english Frame rate(greater than 0)
|
||||
unsigned int nQuality; ///< [IN] \~chinese 视频质量(1-100) \~english Video quality(1-100)
|
||||
IMV_EVideoType recordFormat; ///< [IN] \~chinese 视频格式 \~english Video format
|
||||
const char* pRecordFilePath; ///< [IN] \~chinese 保存视频路径 \~english Save video path
|
||||
unsigned int nReserved[5]; ///< \~chinese 预留 \~english Reserved
|
||||
}IMV_RecordParam;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 录像用帧信息结构体
|
||||
/// \~english
|
||||
/// \brief Frame information for recording structure
|
||||
typedef struct _IMV_RecordFrameInfoParam
|
||||
{
|
||||
unsigned char* pData; ///< [IN] \~chinese 图像数据 \~english Image data
|
||||
unsigned int nDataLen; ///< [IN] \~chinese 图像数据长度 \~english Image data length
|
||||
unsigned int nPaddingX; ///< [IN] \~chinese 图像宽填充 \~english Padding X
|
||||
unsigned int nPaddingY; ///< [IN] \~chinese 图像高填充 \~english Padding Y
|
||||
IMV_EPixelType ePixelFormat; ///< [IN] \~chinese 像素格式 \~english Pixel format
|
||||
unsigned int nReserved[5]; ///< \~chinese 预留 \~english Reserved
|
||||
}IMV_RecordFrameInfoParam;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 图像翻转结构体
|
||||
/// \~english
|
||||
/// \brief Flip image structure
|
||||
typedef struct _IMV_FlipImageParam
|
||||
{
|
||||
unsigned int nWidth; ///< [IN] \~chinese 图像宽 \~english Width
|
||||
unsigned int nHeight; ///< [IN] \~chinese 图像高 \~english Height
|
||||
IMV_EPixelType ePixelFormat; ///< [IN] \~chinese 像素格式 \~english Pixel format
|
||||
IMV_EFlipType eFlipType; ///< [IN] \~chinese 翻转类型 \~english Flip type
|
||||
unsigned char* pSrcData; ///< [IN] \~chinese 输入图像数据 \~english Input image data
|
||||
unsigned int nSrcDataLen; ///< [IN] \~chinese 输入图像长度 \~english Input image length
|
||||
unsigned char* pDstBuf; ///< [OUT] \~chinese 输出数据缓存(调用者分配缓存) \~english Output data buffer(allocated cache by the caller)
|
||||
unsigned int nDstBufSize; ///< [IN] \~chinese 提供的输出缓冲区大小 \~english Provided output buffer size
|
||||
unsigned int nDstDataLen; ///< [OUT] \~chinese 输出数据长度 \~english Output data length
|
||||
unsigned int nReserved[8]; ///< \~chinese 预留 \~english Reserved
|
||||
}IMV_FlipImageParam;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 图像旋转结构体
|
||||
/// \~english
|
||||
/// \brief Rotate image structure
|
||||
typedef struct _IMV_RotateImageParam
|
||||
{
|
||||
unsigned int nWidth; ///< [IN][OUT] \~chinese 图像宽 \~english Width
|
||||
unsigned int nHeight; ///< [IN][OUT] \~chinese 图像高 \~english Height
|
||||
IMV_EPixelType ePixelFormat; ///< [IN] \~chinese 像素格式 \~english Pixel format
|
||||
IMV_ERotationAngle eRotationAngle; ///< [IN] \~chinese 旋转角度 \~english Rotation angle
|
||||
unsigned char* pSrcData; ///< [IN] \~chinese 输入图像数据 \~english Input image data
|
||||
unsigned int nSrcDataLen; ///< [IN] \~chinese 输入图像长度 \~english Input image length
|
||||
unsigned char* pDstBuf; ///< [OUT] \~chinese 输出数据缓存(调用者分配缓存) \~english Output data buffer(allocated cache by the caller)
|
||||
unsigned int nDstBufSize; ///< [IN] \~chinese 提供的输出缓冲区大小 \~english Provided output buffer size
|
||||
unsigned int nDstDataLen; ///< [OUT] \~chinese 输出数据长度 \~english Output data length
|
||||
unsigned int nReserved[8]; ///< \~chinese 预留 \~english Reserved
|
||||
}IMV_RotateImageParam;
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 设备连接状态事件回调函数声明
|
||||
/// \param pParamUpdateArg [in] 回调时主动推送的设备连接状态事件信息
|
||||
/// \param pUser [in] 用户自定义数据
|
||||
/// \~english
|
||||
/// \brief Call back function declaration of device connection status event
|
||||
/// \param pStreamArg [in] The device connection status event which will be active pushed out during the callback
|
||||
/// \param pUser [in] User defined data
|
||||
typedef void(*IMV_ConnectCallBack)(const IMV_SConnectArg* pConnectArg, void* pUser);
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 参数更新事件回调函数声明
|
||||
/// \param pParamUpdateArg [in] 回调时主动推送的参数更新事件信息
|
||||
/// \param pUser [in] 用户自定义数据
|
||||
/// \~english
|
||||
/// \brief Call back function declaration of parameter update event
|
||||
/// \param pStreamArg [in] The parameter update event which will be active pushed out during the callback
|
||||
/// \param pUser [in] User defined data
|
||||
typedef void(*IMV_ParamUpdateCallBack)(const IMV_SParamUpdateArg* pParamUpdateArg, void* pUser);
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 流事件回调函数声明
|
||||
/// \param pStreamArg [in] 回调时主动推送的流事件信息
|
||||
/// \param pUser [in] 用户自定义数据
|
||||
/// \~english
|
||||
/// \brief Call back function declaration of stream event
|
||||
/// \param pStreamArg [in] The stream event which will be active pushed out during the callback
|
||||
/// \param pUser [in] User defined data
|
||||
typedef void(*IMV_StreamCallBack)(const IMV_SStreamArg* pStreamArg, void* pUser);
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 消息通道事件回调函数声明
|
||||
/// \param pMsgChannelArg [in] 回调时主动推送的消息通道事件信息
|
||||
/// \param pUser [in] 用户自定义数据
|
||||
/// \~english
|
||||
/// \brief Call back function declaration of message channel event
|
||||
/// \param pMsgChannelArg [in] The message channel event which will be active pushed out during the callback
|
||||
/// \param pUser [in] User defined data
|
||||
typedef void(*IMV_MsgChannelCallBack)(const IMV_SMsgChannelArg* pMsgChannelArg, void* pUser);
|
||||
|
||||
/// \~chinese
|
||||
/// \brief 帧数据信息回调函数声明
|
||||
/// \param pFrame [in] 回调时主动推送的帧信息
|
||||
/// \param pUser [in] 用户自定义数据
|
||||
/// \~english
|
||||
/// \brief Call back function declaration of frame data information
|
||||
/// \param pFrame [in] The frame information which will be active pushed out during the callback
|
||||
/// \param pUser [in] User defined data
|
||||
typedef void(*IMV_FrameCallBack)(IMV_Frame* pFrame, void* pUser);
|
||||
|
||||
#endif // __IMV_DEFINES_H__
|
||||
0
LidarPublisher.cpp
Normal file
0
LidarPublisher.cpp
Normal file
41
ScreenCamera_2024-11-21-20-52-13.json
Normal file
41
ScreenCamera_2024-11-21-20-52-13.json
Normal file
@ -0,0 +1,41 @@
|
||||
{
|
||||
"class_name" : "PinholeCameraParameters",
|
||||
"extrinsic" :
|
||||
[
|
||||
0.82349700295056028,
|
||||
0.51794784004159533,
|
||||
0.23147941836735964,
|
||||
0.0,
|
||||
-0.55055775397237539,
|
||||
0.82806754878624445,
|
||||
0.10578418685245053,
|
||||
0.0,
|
||||
-0.1368899034711418,
|
||||
-0.21455574949972034,
|
||||
0.96707134415423301,
|
||||
0.0,
|
||||
-2.9882837950628538,
|
||||
-0.94906622954748698,
|
||||
10.63537376453692,
|
||||
1.0
|
||||
],
|
||||
"intrinsic" :
|
||||
{
|
||||
"height" : 1080,
|
||||
"intrinsic_matrix" :
|
||||
[
|
||||
935.30743608719376,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
935.30743608719376,
|
||||
0.0,
|
||||
959.5,
|
||||
539.5,
|
||||
1.0
|
||||
],
|
||||
"width" : 1920
|
||||
},
|
||||
"version_major" : 1,
|
||||
"version_minor" : 0
|
||||
}
|
||||
41
ScreenCamera_2024-11-25-22-24-01.json
Normal file
41
ScreenCamera_2024-11-25-22-24-01.json
Normal file
@ -0,0 +1,41 @@
|
||||
{
|
||||
"class_name" : "PinholeCameraParameters",
|
||||
"extrinsic" :
|
||||
[
|
||||
0.939223094717625,
|
||||
0.062346439634526594,
|
||||
0.33759872602535923,
|
||||
0.0,
|
||||
0.11366480802410098,
|
||||
-0.98438229385923792,
|
||||
-0.13443143588189957,
|
||||
0.0,
|
||||
0.32394488692660589,
|
||||
0.16463420361918521,
|
||||
-0.93164118051575262,
|
||||
0.0,
|
||||
-5.5072699140410242,
|
||||
0.46811610616831523,
|
||||
12.42305128653331,
|
||||
1.0
|
||||
],
|
||||
"intrinsic" :
|
||||
{
|
||||
"height" : 1080,
|
||||
"intrinsic_matrix" :
|
||||
[
|
||||
935.30743608719376,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
935.30743608719376,
|
||||
0.0,
|
||||
959.5,
|
||||
539.5,
|
||||
1.0
|
||||
],
|
||||
"width" : 1920
|
||||
},
|
||||
"version_major" : 1,
|
||||
"version_minor" : 0
|
||||
}
|
||||
209
api_alg.py
Normal file
209
api_alg.py
Normal file
@ -0,0 +1,209 @@
|
||||
#Import modules
|
||||
import cv2
|
||||
import numpy as np
|
||||
import math
|
||||
import time
|
||||
import sys
|
||||
import json
|
||||
|
||||
# 1、src 放标准图还是待检测图
|
||||
# 2、问题一：两张图的尺寸要一致
|
||||
# 3、问题二：相机的z轴 是手臂的y轴
|
||||
# 4、所有参数应该是 第二个参数到第一个参数的差值::标准图必须做目标图像
|
||||
# 5、if m.distance < 0.72*n.distance: 这个阈值是个关键参数，越小定位约精准
|
||||
# 6、assert(isRotationMatrix(R)) 报错是因为截图或者拉伸的原因，使得角度旋转无解
|
||||
# 7、截图比例会影响旋转矩阵，甚至会使得旋转矩阵无解。一定要4:3
|
||||
# 8、原图中目标图像的像素数量与目标图像的像素数量，【缩放比】越接近，M的值越准确
|
||||
# 9、缩放比需要打印
|
||||
# 10、会改变M的参数变量包括：
|
||||
# img1 = cv2.resize
|
||||
# interpolation=cv2.INTER_AREA
|
||||
# cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 1.0) 1.0
|
||||
# if m.distance < 0.85 * n.distance: 0.85
|
||||
# 11、编写自动化选择参数脚本！！！！自动找到最优解
|
||||
# 12、可优化参数：
|
||||
# cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
|
||||
# cv2.resize
|
||||
|
||||
########################################################################
|
||||
#Import pictures
|
||||
homo = "./image/2_origin_centor.jpg"
|
||||
homo_train = "./image/10_bigorigin_rz_x.jpg"
|
||||
|
||||
homo = "./scp_file//o.jpg"
|
||||
homo_train = "./scp_file/6_-0.59803_0.264184_0.588617_-1.556_0.343_1.072.jpg"
|
||||
|
||||
def cal_homo(homo_train, homo):
|
||||
|
||||
start = time.time()
|
||||
# interpolation=cv2.INTER_AREA 必须加
|
||||
img1 = cv2.imread(homo_train, cv2.IMREAD_GRAYSCALE)
|
||||
img1 = cv2.resize(img1, (int(img1.shape[1] / 4), int(img1.shape[0] / 4)), interpolation=cv2.INTER_AREA) # 必须加
|
||||
img2 = cv2.imread(homo, cv2.IMREAD_GRAYSCALE)
|
||||
img2 = cv2.resize(img2, (img1.shape[1], img1.shape[0]), interpolation=cv2.INTER_AREA) # 必须加，两张照片必须同样尺寸
|
||||
|
||||
#Feature Extraction
|
||||
MIN_MATCH_COUNT = 20 ##修改
|
||||
sift = cv2.xfeatures2d.SIFT_create()
|
||||
|
||||
kp1, des1 = sift.detectAndCompute(img1, None)
|
||||
kp2, des2 = sift.detectAndCompute(img2, None)
|
||||
|
||||
FLANN_INDEX_KDTREE = 0
|
||||
|
||||
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
|
||||
search_params = dict(checks = 50)
|
||||
|
||||
flann = cv2.FlannBasedMatcher(index_params, search_params)
|
||||
|
||||
matches = flann.knnMatch(des1, des2, k=2)
|
||||
|
||||
# store all the good matches as per Lowe's ratio test.
|
||||
good = []
|
||||
for m,n in matches:
|
||||
if m.distance < 0.85 * n.distance:
|
||||
good.append(m)
|
||||
|
||||
if len(good)>MIN_MATCH_COUNT:
|
||||
src_pts = np.float32([ kp1[m.queryIdx].pt for m in good ]).reshape(-1,1,2)
|
||||
dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good ]).reshape(-1,1,2)
|
||||
|
||||
|
||||
#Finds homography
|
||||
# M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 1.0) ## 修改 1.0 -> 5.0
|
||||
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 3, maxIters=2000, confidence=0.99) ## 修改 1.0 -> 5.0
|
||||
print("api_alg 单应性矩阵 M:\n", M)
|
||||
|
||||
matchesMask = mask.ravel().tolist()
|
||||
|
||||
h,w = img1.shape
|
||||
pts = np.float32([ [0,0], [0,h-1], [w-1,h-1], [w-1,0] ]).reshape(-1, 1, 2)
|
||||
dst = cv2.perspectiveTransform(pts, M)
|
||||
|
||||
img2 = cv2.polylines(img2, [np.int32(dst)], True, 255, 3, cv2.LINE_AA)
|
||||
|
||||
else:
|
||||
# print ("api_alg : Not enough matches are found - %d/%d") % (len(good), MIN_MATCH_COUNT)
|
||||
matchesMask = None
|
||||
|
||||
# for draw
|
||||
# draw_params = dict(matchColor = (0,255,0), # draw matches in green color
|
||||
# singlePointColor = None,
|
||||
# matchesMask = matchesMask, # draw only inliers
|
||||
# flags = 2)
|
||||
# img3 = cv2.drawMatches(img1, kp1, img2, kp2, good, None, **draw_params)
|
||||
# plt.imshow(img3, 'gray'),plt.show()
|
||||
|
||||
# camera pose from H whitout K
|
||||
def cameraPoseFromHomography(H):
|
||||
norm1 = np.linalg.norm(H[:, 0])
|
||||
norm2 = np.linalg.norm(H[:, 1])
|
||||
tnorm = (norm1 + norm2) / 2.0
|
||||
H1 = H[:, 0] / norm1
|
||||
H2 = H[:, 1] / norm2
|
||||
H3 = np.cross(H1, H2)
|
||||
T = H[:, 2] / tnorm
|
||||
return np.array([H1, H2, H3, T]).transpose()
|
||||
|
||||
RT = cameraPoseFromHomography(M)
|
||||
# print("api_alg RT whitout K", RT)
|
||||
|
||||
focal_length = img1.shape[1] # 使用摄像头的宽度(像素)代表焦距
|
||||
center = (img1.shape[1] / 2, img1.shape[0] / 2) # 使用图像的中心代替光学中心
|
||||
K = np.array([[focal_length, 0, center[0]],
|
||||
[0, focal_length, center[1]],
|
||||
[0, 0, 1]])
|
||||
|
||||
# camera pose from H whit K
|
||||
num, Rs, Ts, Ns = cv2.decomposeHomographyMat(M, K)
|
||||
# print("api_alg Rs", Rs)
|
||||
# print("api_alg Ts", Ts)
|
||||
|
||||
# Checks if a matrix is a valid rotation matrix.
|
||||
def isRotationMatrix(R) :
|
||||
Rt = np.transpose(R)
|
||||
shouldBeIdentity = np.dot(Rt, R)
|
||||
I = np.identity(3, dtype = R.dtype)
|
||||
n = np.linalg.norm(I - shouldBeIdentity)
|
||||
return n < 1e-6
|
||||
|
||||
# Calculates rotation matrix to euler angles
|
||||
# The result is the same as MATLAB except the order
|
||||
# of the euler angles ( x and z are swapped ).
|
||||
def rotationMatrixToEulerAngles(R) :
|
||||
|
||||
# assert(isRotationMatrix(R))
|
||||
if not isRotationMatrix(R):
|
||||
# print("api_alg : assert(isRotationMatrix(R)) !!!!")
|
||||
return np.array([0, 0, 0])
|
||||
|
||||
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
|
||||
|
||||
singular = sy < 1e-6
|
||||
|
||||
if not singular :
|
||||
x = math.atan2(R[2,1] , R[2,2])
|
||||
y = math.atan2(-R[2,0], sy)
|
||||
z = math.atan2(R[1,0], R[0,0])
|
||||
else :
|
||||
x = math.atan2(-R[1,2], R[1,1])
|
||||
y = math.atan2(-R[2,0], sy)
|
||||
z = 0
|
||||
|
||||
return np.array([x, y, z])
|
||||
|
||||
# Conver the 4 rotation matrix solutions into XYZ Euler angles
|
||||
# 获取最后一组值
|
||||
i = 0
|
||||
for i in range(0, len(Rs)):
|
||||
# 获取最后一组值
|
||||
angles = rotationMatrixToEulerAngles(Rs[i])
|
||||
x = np.degrees(angles[0])
|
||||
y = np.degrees(angles[1])
|
||||
z = np.degrees(angles[2])
|
||||
anglesDeg = np.array([x,y,z])
|
||||
# print("api_alg 角度: ", anglesDeg)
|
||||
|
||||
R = Rs[num - 1]
|
||||
T = Ts[num - 1]
|
||||
|
||||
# print("api_alg : Rs矩阵")
|
||||
# for i in range(len(Rs)): print(Rs[i])
|
||||
# print("api_alg : Ts矩阵\n", Ts)
|
||||
|
||||
rand = [x / 180 * math.pi, y / 180 * math.pi, z / 180 * math.pi]
|
||||
|
||||
print("api_alg 图像计算旋转矩阵 R: \n", R)
|
||||
print("api_alg 图像计算角度偏移 a: ", anglesDeg)
|
||||
print("api_alg 图像计算弧度偏移 r: ", rand)
|
||||
print("api_alg 图像计算平移偏移 t: ", [T[0][0], T[1][0], T[2][0]])
|
||||
|
||||
end = time.time()
|
||||
print("api_alg 程序的运行时间为：{}".format(end-start))
|
||||
print(rand[0], rand[1], rand[2])
|
||||
|
||||
return rand[0], rand[1], rand[2], M[0][0], M[1][1], M[0][2], M[1][2], R, T
|
||||
|
||||
if __name__ == '__main__':
|
||||
resp = {'code': 0, 'message': 'success'}
|
||||
if len(sys.argv) != 3:
|
||||
resp['code'] = -1
|
||||
resp['message'] = '参数错误！'
|
||||
else:
|
||||
img1 = sys.argv[1]
|
||||
img2 = sys.argv[2]
|
||||
result = cal_homo(img1, img2)
|
||||
resp['result'] = {
|
||||
'drx': result[0],
|
||||
'dry': result[1],
|
||||
'drz': result[2],
|
||||
'hs': result[3],
|
||||
'ws': result[4],
|
||||
'xf': result[5],
|
||||
'yf': result[6],
|
||||
'dx': result[8][0][0],
|
||||
'dy': result[8][1][0],
|
||||
'dz': result[8][2][0],
|
||||
}
|
||||
|
||||
print(json.dumps(resp, indent = 0, ensure_ascii = False))
|
||||
332
api_alg_pnp.py
Normal file
332
api_alg_pnp.py
Normal file
@ -0,0 +1,332 @@
|
||||
#Import modules
|
||||
import os
|
||||
from unicodedata import mirrored
|
||||
import cv2
|
||||
import numpy as np
|
||||
import math
|
||||
import time
|
||||
import json
|
||||
import sys
|
||||
# import transforms3d as tfs
|
||||
|
||||
# 1、src 放标准图还是待检测图
|
||||
# 2、问题一：两张图的尺寸要一致
|
||||
# 3、问题二：相机的z轴 是手臂的y轴
|
||||
# 4、所有参数应该是 第二个参数到第一个参数的差值::标准图必须做目标图像
|
||||
# 5、if m.distance < 0.72*n.distance: 这个阈值是个关键参数，越小定位约精准
|
||||
# 6、assert(isRotationMatrix(R)) 报错是因为截图或者拉伸的原因，使得角度旋转无解
|
||||
# 7、截图比例会影响旋转矩阵，甚至会使得旋转矩阵无解。一定要4:3
|
||||
# 8、原图中目标图像的像素数量与目标图像的像素数量，【缩放比】越接近，M的值越准确
|
||||
# 9、缩放比需要打印
|
||||
# 10、会改变M的参数变量包括：
|
||||
# img1 = cv2.resize
|
||||
# interpolation=cv2.INTER_AREA
|
||||
# cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 1.0) 1.0
|
||||
# if m.distance < 0.85 * n.distance: 0.85
|
||||
# 11、编写自动化选择参数脚本！！！！自动找到最优解
|
||||
# 12、可优化参数：
|
||||
# cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
|
||||
# cv2.resize
|
||||
|
||||
########################################################################
|
||||
#Import pictures
|
||||
homo = "./image/2_origin_centor.jpg"
|
||||
homo_train = "./image/10_bigorigin_rz_x.jpg"
|
||||
|
||||
homo = "./scp_file//o.jpg"
|
||||
homo_train = "./scp_file/6_-0.59803_0.264184_0.588617_-1.556_0.343_1.072.jpg"
|
||||
|
||||
homo = "./error1009/grayscale1.jpg"
|
||||
homo_train = "./error1009/9983823d/GRAY_SCALE/WIDE/1/IMG_20221009_105934.jpg"
|
||||
|
||||
# homo = "./error0926/c24.jpg"
|
||||
# homo = "./error0928/1g.jpg"
|
||||
# homo_train = "./error0926/IMG_20220926_165931.jpg"
|
||||
|
||||
homo = "./error1010/sfrplus.jpg"
|
||||
homo_train = "./error1010/IMG_20221010_162855.jpg"
|
||||
|
||||
# homo_train = "./060701/IMG_20230604_130953.jpg"
|
||||
# homo = "./error06042/C24.jpg"
|
||||
# homo_train = "./060701/IMG_20230603_185437.jpg"
|
||||
# homo_train = "./060701/IMG_20230603_185512.jpg"
|
||||
# homo_train = "./060701/IMG_20230603_185518.jpg"
|
||||
# homo_train = "./error0604/IMG_20230604_175620.jpg"
|
||||
# homo_train = "./error0604/IMG_20230604_175620.jpg"
|
||||
# homo_train = "./error06042/IMG_20230604_182406.jpg"
|
||||
|
||||
|
||||
homo = "./error0605/grayscale.jpg"
|
||||
homo_train = "./error0605/IMG_20230605_121151.jpg"
|
||||
|
||||
# homo = "./error0605/sfrplus.jpg"
|
||||
# homo_train = "./error0605/IMG_20230605_132952.jpg"
|
||||
|
||||
# homo = "./error0605 near/sfrplus.jpg"
|
||||
# homo_train = "./error0605 near/IMG_20230605_174504.jpg"
|
||||
|
||||
homo = "./error0605gray/grayscale.jpg"
|
||||
homo_train = "./error0605gray/IMG_20230605_194543.jpg"
|
||||
|
||||
|
||||
def Pnp(objpos, imgpos, M, cameraMatrix, coff_dis):
|
||||
|
||||
dst_imgpos = cv2.perspectiveTransform(imgpos, M)
|
||||
# imgp.append(dst_imgpos)
|
||||
# pnp 求解
|
||||
_, rvec, tvec = cv2.solvePnP(objpos, dst_imgpos, cameraMatrix, None, flags=cv2.SOLVEPNP_UPNP)
|
||||
# print(cv2.Rodrigues(rvec))
|
||||
rotM = cv2.Rodrigues(rvec)[0]
|
||||
# camera_postion = -np.mat(rotM).T * np.mat(tvec)
|
||||
# Ca = camera_postion.T
|
||||
|
||||
# 计算相机坐标系的三轴旋转欧拉角，旋转后可以转出世界坐标系。旋转顺序z,y,x
|
||||
thetaZ = math.atan2(rotM[1, 0], rotM[0, 0]) * 180.0 / math.pi
|
||||
thetaY = math.atan2(-1.0 * rotM[2, 0], math.sqrt(rotM[2, 1] ** 2 + rotM[2, 2] ** 2)) * 180.0 / math.pi
|
||||
thetaX = math.atan2(rotM[2, 1], rotM[2, 2]) * 180.0 / math.pi
|
||||
|
||||
# 相机坐标系下值
|
||||
x = tvec[0]
|
||||
y = tvec[1]
|
||||
z = tvec[2]
|
||||
|
||||
# 进行三次旋转
|
||||
def RotateByZ(Cx, Cy, thetaZ):
|
||||
rz = thetaZ * math.pi / 180.0
|
||||
outX = math.cos(rz) * Cx - math.sin(rz) * Cy
|
||||
outY = math.sin(rz) * Cx + math.cos(rz) * Cy
|
||||
return outX, outY
|
||||
|
||||
def RotateByY(Cx, Cz, thetaY):
|
||||
ry = thetaY * math.pi / 180.0
|
||||
outZ = math.cos(ry) * Cz - math.sin(ry) * Cx
|
||||
outX = math.sin(ry) * Cz + math.cos(ry) * Cx
|
||||
return outX, outZ
|
||||
|
||||
def RotateByX(Cy, Cz, thetaX):
|
||||
rx = thetaX * math.pi / 180.0
|
||||
outY = math.cos(rx) * Cy - math.sin(rx) * Cz
|
||||
outZ = math.sin(rx) * Cy + math.cos(rx) * Cz
|
||||
return outY, outZ
|
||||
|
||||
(x, y) = RotateByZ(x, y, -1.0 * thetaZ)
|
||||
(x, z) = RotateByY(x, z, -1.0 * thetaY)
|
||||
(y, z) = RotateByX(y, z, -1.0 * thetaX)
|
||||
Cx = x * -1
|
||||
Cy = y * -1
|
||||
Cz = z * -1
|
||||
|
||||
# 输出相机位置
|
||||
camera_Location = [Cx, Cy, Cz]
|
||||
# print("相机位姿",camera_Location)
|
||||
|
||||
# 输出相机旋转角
|
||||
# print("相机旋转角度：",'[', thetaX, thetaY, -thetaZ, ']')
|
||||
|
||||
angles = [thetaX, thetaY, thetaZ]
|
||||
#result = [camera_Location, "旋转角度", angles]
|
||||
|
||||
return angles, camera_Location
|
||||
|
||||
|
||||
# 欧拉角->旋转矩阵
|
||||
def eulerAnglesToRotationMatrix(theta) :
|
||||
R_x = np.array([[1, 0, 0 ],
|
||||
[0, math.cos(theta[0]), -math.sin(theta[0]) ],
|
||||
[0, math.sin(theta[0]), math.cos(theta[0]) ]
|
||||
])
|
||||
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ],
|
||||
[0, 1, 0 ],
|
||||
[-math.sin(theta[1]), 0, math.cos(theta[1]) ]
|
||||
])
|
||||
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
|
||||
[math.sin(theta[2]), math.cos(theta[2]), 0],
|
||||
[0, 0, 1]
|
||||
])
|
||||
R = np.dot(R_z, np.dot( R_y, R_x ))
|
||||
return R
|
||||
|
||||
|
||||
def _angel_zoom(rx, ry, rz, angel_zoom=1):
|
||||
ax, angle = tfs.euler.euler2axangle(rx, ry, rz, "sxyz")
|
||||
eulers = tfs.euler.axangle2euler(ax * angel_zoom, angle * angel_zoom, "sxyz")
|
||||
return eulers[0], eulers[1], eulers[2]
|
||||
|
||||
|
||||
# 固定 resolution = 1， sigma = 1.9
|
||||
def cal_homo(homo_train, homo, mirror = False, resolution = 1, angle_zoom=1, sigma = 1.6):
|
||||
|
||||
# 默认初始化参数
|
||||
resolution = 1
|
||||
|
||||
angle_zoom = 1
|
||||
|
||||
# SIFT:该值越大，被选中的特征点越多，计算时间越长，推荐10 同时适用于广角和后置
|
||||
edgeThreshold = 10
|
||||
|
||||
# SIFT:该值越大，精度越高，计算时间越长
|
||||
sigma = 4.5
|
||||
# if os.path.basename(homo) == "grayscale.jpg": sigma = 1.9
|
||||
|
||||
ransacReprojThreshold = 3.0 # Homography: 该值越小，精度越高，对时间没有影像
|
||||
|
||||
# imgWidth、imgHeight 越大，精度越高，计算时间越长
|
||||
imgWidth = 1000
|
||||
imgHeight = 750
|
||||
# if os.path.basename(homo) == "grayscale.jpg":
|
||||
# imgWidth = 1600
|
||||
# imgHeight = 1200
|
||||
|
||||
# 算法开始执行计时
|
||||
start = time.time()
|
||||
|
||||
# 读取图像与标准图像进行对比
|
||||
img1 = cv2.imread(homo_train, cv2.IMREAD_GRAYSCALE)
|
||||
if mirror: img1 = cv2.flip(img1, 1)
|
||||
|
||||
img1 = cv2.resize(img1, (imgWidth, imgHeight), interpolation=cv2.INTER_AREA) # 使用INTER_AREA可以提升特征点识别率
|
||||
img2 = cv2.imread(homo, cv2.IMREAD_GRAYSCALE)
|
||||
img2 = cv2.resize(img2, (img1.shape[1], img1.shape[0]), interpolation=cv2.INTER_AREA) # 使用INTER_AREA可以提升特征点识别率，两张照片必须同样尺寸
|
||||
|
||||
|
||||
# 1、使用 SIFT 进行特征提取
|
||||
MIN_MATCH_COUNT = 20
|
||||
MAX_MATCH_COUNT = 100
|
||||
|
||||
# 从优选择特征点，限定800个
|
||||
if resolution == 1: sift = cv2.xfeatures2d.SIFT_create(nfeatures=1500, nOctaveLayers=10, contrastThreshold=0.02, edgeThreshold=edgeThreshold, sigma=sigma)
|
||||
else: sift = cv2.xfeatures2d.SIFT_create(nfeatures=1500)
|
||||
|
||||
kp1, des1 = sift.detectAndCompute(img1, None)
|
||||
kp2, des2 = sift.detectAndCompute(img2, None)
|
||||
|
||||
|
||||
# 2、使用 flann 进行特征点匹配
|
||||
FLANN_INDEX_KDTREE = 1
|
||||
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
|
||||
search_params = dict(checks = 50)
|
||||
flann = cv2.FlannBasedMatcher(index_params, search_params)
|
||||
matches = flann.knnMatch(des1, des2, k=2)
|
||||
|
||||
|
||||
# 挑选匹配情况较好的特征点
|
||||
good = []
|
||||
for m,n in matches:
|
||||
if m.distance < 0.85 * n.distance:
|
||||
good.append(m)
|
||||
# 从优选择特征点，限定MAX个特征
|
||||
# good = sorted(good, key=lambda x: x.distance)
|
||||
# if len(good) >= MAX_MATCH_COUNT: good = good[:100]
|
||||
|
||||
|
||||
# 如果特征匹配点的数量不低于 MIN_MATCH_COUNT 设置的数量
|
||||
if len(good) > MIN_MATCH_COUNT:
|
||||
src_pts = np.float32([ kp1[m.queryIdx].pt for m in good ]).reshape(-1,1,2)
|
||||
dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good ]).reshape(-1,1,2)
|
||||
|
||||
# 3、查找单应矩阵
|
||||
# 修改第三个参数 1.0 -> 5.0
|
||||
# 利用基于RANSAC的鲁棒算法选择最优的四组配对点，再计算转换矩阵H(3*3)并返回,以便于反向投影错误率达到最小
|
||||
# ransacReprojThreshold 设置为 1.0，精度最高
|
||||
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, ransacReprojThreshold, maxIters=1500, confidence=0.998) ## 修改 1.0 -> 5.0
|
||||
# print("api_alg 单应性矩阵 M:\n", M)
|
||||
matchesMask = mask.ravel().tolist()
|
||||
|
||||
|
||||
# 用于测试显示，使用时注释
|
||||
import matplotlib.pyplot as plt
|
||||
plt.ion()
|
||||
draw_params = dict(matchColor = (0,255,0),
|
||||
singlePointColor = None,
|
||||
matchesMask = matchesMask,
|
||||
flags = 2)
|
||||
img3 = cv2.drawMatches(img1, kp1, img2, kp2, good, None, **draw_params)
|
||||
plt.imshow(img3, 'gray'),plt.show()
|
||||
|
||||
|
||||
# 4、采用PNP进行相机的姿态估计
|
||||
h, w = img1.shape
|
||||
|
||||
# 模拟实际物体世界坐标参数
|
||||
if resolution == 1:
|
||||
objpos = np.float32([[0.001, 0.001, 0], [0.001, 1.200, 0], [1.600, 1.200, 0], [1.600, 0.001, 0]]).reshape(-1, 1, 3)
|
||||
else:
|
||||
objpos = np.float32([[0.0008, 0.0008, 0], [0.0008, 0.5994, 0], [0.7992, 0.5994, 0], [0.7992, 0.0008, 0]]).reshape(-1, 1, 3)
|
||||
imgpos = np.float32([[0,0], [0,h-1], [w-1,h-1], [w-1,0]]).reshape(-1, 1, 2)
|
||||
|
||||
# 模拟相机参数
|
||||
focal_length = img1.shape[1] # 使用摄像头的宽度(像素)代表焦距
|
||||
center = (img1.shape[1] / 2, img1.shape[0] / 2) # 使用图像的中心代替光学中心
|
||||
K = np.array([[focal_length, 0, center[0]],
|
||||
[0, focal_length, center[1]],
|
||||
[0, 0, 1]])
|
||||
|
||||
angles, T = Pnp(objpos, imgpos, M, K, None)
|
||||
rand = [angles[0] / 180 * math.pi, angles[1] / 180 * math.pi, angles[2] / 180 * math.pi]
|
||||
R = eulerAnglesToRotationMatrix(angles)
|
||||
# print("api_alg 图像计算旋转矩阵 R: \n", R)
|
||||
# print("api_alg 图像计算角度偏移 a: ", angles)
|
||||
# print("api_alg 图像计算弧度偏移 r: ", rand)
|
||||
# print("api_alg 图像计算平移偏移 t: ", [[T[0][0]], [T[1][0]], [T[2][0]]])
|
||||
# print("mmatches count: ", len(matches), " good count: ", len(good), " matchesMask count: ", len(matchesMask))
|
||||
# print(rand[0], rand[1], rand[2], M[0][0], M[1][1], M[0][2], M[1][2])
|
||||
print("mmatches count: ", len(matches), " good count: ", len(good), " matchesMask count: ", len(matchesMask))
|
||||
print(rand[0], rand[1], rand[2], M[0][0], M[1][1], M[0][2], M[1][2])
|
||||
|
||||
end = time.time()
|
||||
print("api_alg 程序的运行时间为：{}".format(end-start))
|
||||
# print(rand[0], rand[1], rand[2])
|
||||
if angle_zoom != 1:
|
||||
rand[0], rand[1], rand[2] = _angel_zoom(rand[0], rand[1], rand[2], angle_zoom)
|
||||
return rand[0], rand[1], rand[2], M[0][0], M[1][1], M[0][2], M[1][2], R, T
|
||||
# img2 = cv2.polylines(img2, [np.int32(dst)], True, 255, 3, cv2.LINE_AA)
|
||||
|
||||
else:
|
||||
# print ("api_alg : Not enough matches are found - %d/%d") % (len(good), MIN_MATCH_COUNT)
|
||||
matchesMask = None
|
||||
# print("good features is too few!", "mmatches count: ", len(matches), " good count: ", len(good))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
cal_homo(homo_train, homo)
|
||||
|
||||
# resp = {'code': 0, 'message': 'success'}
|
||||
# if len(sys.argv) < 3:
|
||||
# resp['code'] = -1
|
||||
# resp['message'] = '参数错误！'
|
||||
# else:
|
||||
# img1 = sys.argv[1]
|
||||
# img2 = sys.argv[2]
|
||||
# mirror = False
|
||||
# resolution = 0
|
||||
# angle_zoom = 1
|
||||
# sigma = 1.3
|
||||
# if len(sys.argv) >= 4:
|
||||
# mirror = sys.argv[3].lower() == 'true'
|
||||
# if len(sys.argv) >= 5:
|
||||
# resolution = int(sys.argv[4])
|
||||
# if len(sys.argv) >= 6:
|
||||
# angle_zoom = float(sys.argv[5])
|
||||
# if len(sys.argv) >= 7:
|
||||
# sigma = float(sys.argv[6])
|
||||
|
||||
# result = cal_homo(img1, img2, mirror, resolution, angle_zoom, sigma)
|
||||
|
||||
# if result is not None:
|
||||
# resp['result'] = {
|
||||
# 'drx': result[0],
|
||||
# 'dry': result[1],
|
||||
# 'drz': result[2],
|
||||
# 'hs': result[3],
|
||||
# 'ws': result[4],
|
||||
# 'xf': result[5],
|
||||
# 'yf': result[6],
|
||||
# 'dx': result[8][0][0],
|
||||
# 'dy': result[8][1][0],
|
||||
# 'dz': result[8][2][0],
|
||||
# }
|
||||
# else:
|
||||
# resp['code'] = -1
|
||||
# resp['message'] = '解析错误！'
|
||||
|
||||
# print(json.dumps(resp, indent = 2, ensure_ascii = False))
|
||||
12
config_file.txt
Normal file
12
config_file.txt
Normal file
@ -0,0 +1,12 @@
|
||||
9344 7000
|
||||
-2.5 2.5
|
||||
-2.5 2.5
|
||||
0.0 3.8
|
||||
0.000001
|
||||
2
|
||||
0
|
||||
11057.154 0.0 4538.85 0.0
|
||||
0.0 11044.943 3350.918 0.0
|
||||
0.0 0.0 1.0 0.0
|
||||
1.57 -1.57 1.0
|
||||
0
|
||||
97
convert.py
Normal file
97
convert.py
Normal file
@ -0,0 +1,97 @@
|
||||
# -- coding: utf-8 --
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation as rot
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
# 旋转矩阵转旋转向量（Rxyz->Rvector，按特定轴旋转）
|
||||
def rotvector2rot(rotvector):
|
||||
Rm = cv2.Rodrigues(rotvector)[0]
|
||||
return Rm
|
||||
|
||||
# 旋转矩阵转欧拉角
|
||||
def isRotationMatrix(R):
|
||||
Rt = np.transpose(R)
|
||||
shouldBeIdentity = np.dot(Rt, R)
|
||||
I = np.identity(3, dtype=R.dtype)
|
||||
n = np.linalg.norm(I - shouldBeIdentity)
|
||||
return n < 1e-6
|
||||
def rot2euler(R):
|
||||
# assert (isRotationMatrix(R))
|
||||
sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
|
||||
singular = sy < 1e-6
|
||||
if not singular:
|
||||
x = math.atan2(R[2, 1], R[2, 2]) * 180 / np.pi
|
||||
y = math.atan2(-R[2, 0], sy) * 180 / np.pi
|
||||
z = math.atan2(R[1, 0], R[0, 0]) * 180 / np.pi
|
||||
else:
|
||||
x = math.atan2(-R[1, 2], R[1, 1]) * 180 / np.pi
|
||||
y = math.atan2(-R[2, 0], sy) * 180 / np.pi
|
||||
z = 0
|
||||
return np.array([x, y, z])
|
||||
def rot2eulerpi(R):
|
||||
# assert (isRotationMatrix(R))
|
||||
sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
|
||||
singular = sy < 1e-6
|
||||
if not singular:
|
||||
x = math.atan2(R[2, 1], R[2, 2])
|
||||
y = math.atan2(-R[2, 0], sy)
|
||||
z = math.atan2(R[1, 0], R[0, 0])
|
||||
else:
|
||||
x = math.atan2(-R[1, 2], R[1, 1])
|
||||
y = math.atan2(-R[2, 0], sy)
|
||||
z = 0
|
||||
return np.array([x, y, z])
|
||||
def rot_to_ypr_zyx(R):
|
||||
"""
|
||||
将旋转矩阵R转换为Yaw-Pitch-Roll角度（Z-Y-X顺序）
|
||||
"""
|
||||
sin_p = 2 * (R[2, 0] ** 2 - R[0, 0] ** 2 + R[1, 0] ** 2) ** 0.5
|
||||
if sin_p < 1e-6:
|
||||
sin_p = 1e-6
|
||||
if R[2, 0] > 0:
|
||||
sin_p = -sin_p
|
||||
|
||||
roll = np.arctan2(R[2, 0], R[2, 2])
|
||||
pitch = np.arcsin(-R[2, 1])
|
||||
yaw = np.arctan2(R[1, 0], R[0, 0])
|
||||
|
||||
return yaw, pitch, roll
|
||||
def rot_to_ypr_xyz(R):
|
||||
# 计算R的trace值
|
||||
tr = np.trace(R)
|
||||
# 计算sqrt(R.T.R) - 1的模长，即平方根trace值
|
||||
sqrt_tr_m1 = np.sqrt(0.25 * ((tr - 3) ** 2) + (R[2, 2] - 1) ** 2)
|
||||
|
||||
# 根据平方根trace值判断Yaw-Pitch-Roll角的计算方法
|
||||
yaw = np.arctan2(R[2, 1], R[2, 2]) # 使用arctan2计算Yaw角
|
||||
pitch = np.arcsin(-R[2, 0]) # 使用arcsin计算Pitch角
|
||||
roll = np.arctan2(R[1, 0], R[0, 0]) # 使用arctan2计算Roll角
|
||||
|
||||
return yaw, pitch, roll
|
||||
|
||||
# 四元数转欧拉角
|
||||
def quaternion2euler(quaternion):
|
||||
r = rot.from_quat(quaternion)
|
||||
euler = r.as_euler('xyz', degrees=True)
|
||||
return euler
|
||||
|
||||
# 四元数转旋转矩阵
|
||||
def quaternion2rot(quaternion):
|
||||
r = rot.from_quat(quaternion)
|
||||
rot = r.as_matrix()
|
||||
return rot
|
||||
|
||||
# 欧拉角转四元数
|
||||
def euler2quaternion(euler):
|
||||
r = rot.from_euler('xyz', euler, degrees=True)
|
||||
quaternion = r.as_quat()
|
||||
return quaternion
|
||||
|
||||
# 欧拉角转旋转矩阵
|
||||
def euler2rot(euler):
|
||||
r = rot.from_euler('xyz', euler, degrees=True)
|
||||
rotation_matrix = r.as_matrix()
|
||||
return rotation_matrix
|
||||
130
deploy.prototxt
Normal file
130
deploy.prototxt
Normal file
@ -0,0 +1,130 @@
|
||||
name: "FCN"
|
||||
|
||||
input: "data"
|
||||
input_dim: 1
|
||||
input_dim: 1
|
||||
input_dim: 500
|
||||
input_dim: 500
|
||||
|
||||
layer { bottom: 'data' top: 'conv1_1' name: 'conv1_1' type: "Convolution"
|
||||
param { lr_mult: 1 decay_mult: 1 } param { lr_mult: 2 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 64 pad: 35 kernel_size: 3 } }
|
||||
layer { bottom: 'conv1_1' top: 'conv1_1' name: 'relu1_1' type: "ReLU" }
|
||||
layer { bottom: 'conv1_1' top: 'conv1_2' name: 'conv1_2' type: "Convolution"
|
||||
param { lr_mult: 1 decay_mult: 1 } param { lr_mult: 2 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 64 pad: 1 kernel_size: 3 } }
|
||||
layer { bottom: 'conv1_2' top: 'conv1_2' name: 'relu1_2' type: "ReLU" }
|
||||
layer { name: 'pool1' bottom: 'conv1_2' top: 'pool1' type: "Pooling"
|
||||
pooling_param { pool: MAX kernel_size: 2 stride: 2 } }
|
||||
|
||||
layer { name: 'conv2_1' bottom: 'pool1' top: 'conv2_1' type: "Convolution"
|
||||
param { lr_mult: 1 decay_mult: 1 } param { lr_mult: 2 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 128 pad: 1 kernel_size: 3 } }
|
||||
layer { bottom: 'conv2_1' top: 'conv2_1' name: 'relu2_1' type: "ReLU" }
|
||||
layer { bottom: 'conv2_1' top: 'conv2_2' name: 'conv2_2' type: "Convolution"
|
||||
param { lr_mult: 1 decay_mult: 1 } param { lr_mult: 2 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 128 pad: 1 kernel_size: 3 } }
|
||||
layer { bottom: 'conv2_2' top: 'conv2_2' name: 'relu2_2' type: "ReLU" }
|
||||
layer { bottom: 'conv2_2' top: 'pool2' name: 'pool2' type: "Pooling"
|
||||
pooling_param { pool: MAX kernel_size: 2 stride: 2 } }
|
||||
|
||||
layer { bottom: 'pool2' top: 'conv3_1' name: 'conv3_1' type: "Convolution"
|
||||
param { lr_mult: 1 decay_mult: 1 } param { lr_mult: 2 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 256 pad: 1 kernel_size: 3 } }
|
||||
layer { bottom: 'conv3_1' top: 'conv3_1' name: 'relu3_1' type: "ReLU" }
|
||||
layer { bottom: 'conv3_1' top: 'conv3_2' name: 'conv3_2' type: "Convolution"
|
||||
param { lr_mult: 1 decay_mult: 1 } param { lr_mult: 2 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 256 pad: 1 kernel_size: 3 } }
|
||||
layer { bottom: 'conv3_2' top: 'conv3_2' name: 'relu3_2' type: "ReLU" }
|
||||
layer { bottom: 'conv3_2' top: 'conv3_3' name: 'conv3_3' type: "Convolution"
|
||||
param { lr_mult: 1 decay_mult: 1 } param { lr_mult: 2 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 256 pad: 1 kernel_size: 3 } }
|
||||
layer { bottom: 'conv3_3' top: 'conv3_3' name: 'relu3_3' type: "ReLU" }
|
||||
layer { bottom: 'conv3_3' top: 'pool3' name: 'pool3' type: "Pooling"
|
||||
pooling_param { pool: MAX kernel_size: 2 stride: 2 } }
|
||||
|
||||
layer { bottom: 'pool3' top: 'conv4_1' name: 'conv4_1' type: "Convolution"
|
||||
param { lr_mult: 1 decay_mult: 1 } param { lr_mult: 2 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 512 pad: 1 kernel_size: 3 } }
|
||||
layer { bottom: 'conv4_1' top: 'conv4_1' name: 'relu4_1' type: "ReLU" }
|
||||
layer { bottom: 'conv4_1' top: 'conv4_2' name: 'conv4_2' type: "Convolution"
|
||||
param { lr_mult: 1 decay_mult: 1 } param { lr_mult: 2 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 512 pad: 1 kernel_size: 3 } }
|
||||
layer { bottom: 'conv4_2' top: 'conv4_2' name: 'relu4_2' type: "ReLU" }
|
||||
layer { bottom: 'conv4_2' top: 'conv4_3' name: 'conv4_3' type: "Convolution"
|
||||
param { lr_mult: 1 decay_mult: 1 } param { lr_mult: 2 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 512 pad: 1 kernel_size: 3 } }
|
||||
layer { bottom: 'conv4_3' top: 'conv4_3' name: 'relu4_3' type: "ReLU" }
|
||||
layer { bottom: 'conv4_3' top: 'pool4' name: 'pool4' type: "Pooling"
|
||||
pooling_param { pool: MAX kernel_size: 2 stride: 2 } }
|
||||
|
||||
layer { bottom: 'pool4' top: 'conv5_1' name: 'conv5_1' type: "Convolution"
|
||||
param { lr_mult: 100 decay_mult: 1 } param { lr_mult: 200 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 512 pad: 1 kernel_size: 3 } }
|
||||
layer { bottom: 'conv5_1' top: 'conv5_1' name: 'relu5_1' type: "ReLU" }
|
||||
layer { bottom: 'conv5_1' top: 'conv5_2' name: 'conv5_2' type: "Convolution"
|
||||
param { lr_mult: 100 decay_mult: 1 } param { lr_mult: 200 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 512 pad: 1 kernel_size: 3 } }
|
||||
layer { bottom: 'conv5_2' top: 'conv5_2' name: 'relu5_2' type: "ReLU" }
|
||||
layer { bottom: 'conv5_2' top: 'conv5_3' name: 'conv5_3' type: "Convolution"
|
||||
param { lr_mult: 100 decay_mult: 1 } param { lr_mult: 200 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 512 pad: 1 kernel_size: 3 } }
|
||||
layer { bottom: 'conv5_3' top: 'conv5_3' name: 'relu5_3' type: "ReLU" }
|
||||
|
||||
## DSN conv 1 ###
|
||||
layer { name: 'score-dsn1' type: "Convolution" bottom: 'conv1_2' top: 'score-dsn1-up'
|
||||
param { lr_mult: 0.01 decay_mult: 1 } param { lr_mult: 0.02 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 1 kernel_size: 1 } }
|
||||
layer { type: "Crop" name: 'crop' bottom: 'score-dsn1-up' bottom: 'data' top: 'upscore-dsn1' }
|
||||
layer { type: "Sigmoid" name: "sigmoid-dsn1" bottom: "upscore-dsn1" top:"sigmoid-dsn1"}
|
||||
|
||||
### DSN conv 2 ###
|
||||
layer { name: 'score-dsn2' type: "Convolution" bottom: 'conv2_2' top: 'score-dsn2'
|
||||
param { lr_mult: 0.01 decay_mult: 1 } param { lr_mult: 0.02 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 1 kernel_size: 1 } }
|
||||
layer { type: "Deconvolution" name: 'upsample_2' bottom: 'score-dsn2' top: 'score-dsn2-up'
|
||||
param { lr_mult: 0 decay_mult: 1 } param { lr_mult: 0 decay_mult: 0}
|
||||
convolution_param { kernel_size: 4 stride: 2 num_output: 1 } }
|
||||
layer { type: "Crop" name: 'crop' bottom: 'score-dsn2-up' bottom: 'data' top: 'upscore-dsn2' }
|
||||
layer { type: "Sigmoid" name: "sigmoid-dsn2" bottom: "upscore-dsn2" top:"sigmoid-dsn2"}
|
||||
|
||||
### DSN conv 3 ###
|
||||
layer { name: 'score-dsn3' type: "Convolution" bottom: 'conv3_3' top: 'score-dsn3'
|
||||
param { lr_mult: 0.01 decay_mult: 1 } param { lr_mult: 0.02 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 1 kernel_size: 1 } }
|
||||
layer { type: "Deconvolution" name: 'upsample_4' bottom: 'score-dsn3' top: 'score-dsn3-up'
|
||||
param { lr_mult: 0 decay_mult: 1 } param { lr_mult: 0 decay_mult: 0}
|
||||
convolution_param { kernel_size: 8 stride: 4 num_output: 1 } }
|
||||
layer { type: "Crop" name: 'crop' bottom: 'score-dsn3-up' bottom: 'data' top: 'upscore-dsn3' }
|
||||
layer { type: "Sigmoid" name: "sigmoid-dsn3" bottom: "upscore-dsn3" top:"sigmoid-dsn3"}
|
||||
|
||||
###DSN conv 4###
|
||||
layer { name: 'score-dsn4' type: "Convolution" bottom: 'conv4_3' top: 'score-dsn4'
|
||||
param { lr_mult: 0.01 decay_mult: 1 } param { lr_mult: 0.02 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 1 kernel_size: 1 } }
|
||||
layer { type: "Deconvolution" name: 'upsample_8' bottom: 'score-dsn4' top: 'score-dsn4-up'
|
||||
param { lr_mult: 0 decay_mult: 1 } param { lr_mult: 0 decay_mult: 0}
|
||||
convolution_param { kernel_size: 16 stride: 8 num_output: 1 } }
|
||||
layer { type: "Crop" name: 'crop' bottom: 'score-dsn4-up' bottom: 'data' top: 'upscore-dsn4' }
|
||||
layer { type: "Sigmoid" name: "sigmoid-dsn4" bottom: "upscore-dsn4" top:"sigmoid-dsn4"}
|
||||
|
||||
|
||||
###DSN conv 5###
|
||||
layer { name: 'score-dsn5' type: "Convolution" bottom: 'conv5_3' top: 'score-dsn5'
|
||||
param { lr_mult: 0.01 decay_mult: 1 } param { lr_mult: 0.02 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 1 kernel_size: 1 } }
|
||||
layer { type: "Deconvolution" name: 'upsample_16' bottom: 'score-dsn5' top: 'score-dsn5-up'
|
||||
param { lr_mult: 0 decay_mult: 1 } param { lr_mult: 0 decay_mult: 0}
|
||||
convolution_param { kernel_size: 32 stride: 16 num_output: 1 } }
|
||||
layer { type: "Crop" name: 'crop' bottom: 'score-dsn5-up' bottom: 'data' top: 'upscore-dsn5' }
|
||||
layer { type: "Sigmoid" name: "sigmoid-dsn5" bottom: "upscore-dsn5" top:"sigmoid-dsn5"}
|
||||
|
||||
|
||||
### Concat and multiscale weight layer ###
|
||||
layer { name: "concat" bottom: "upscore-dsn1" bottom: "upscore-dsn2" bottom: "upscore-dsn3"
|
||||
bottom: "upscore-dsn4" bottom: "upscore-dsn5" top: "concat-upscore" type: "Concat"
|
||||
concat_param { concat_dim: 1} }
|
||||
layer { name: 'new-score-weighting' type: "Convolution" bottom: 'concat-upscore' top: 'upscore-fuse'
|
||||
param { lr_mult: 0.01 decay_mult: 1 } param { lr_mult: 0.02 decay_mult: 0}
|
||||
convolution_param { engine: CAFFE num_output: 1 kernel_size: 1 weight_filler {type: "constant" value: 0.2} } }
|
||||
layer { type: "Sigmoid" name: "sigmoid-fuse" bottom: "upscore-fuse" top:"sigmoid-fuse"}
|
||||
BIN
distorted_img.png
Normal file
BIN
distorted_img.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 54 MiB |
215
find_velodyne_points_self_define.cpp
Normal file
215
find_velodyne_points_self_define.cpp
Normal file
@ -0,0 +1,215 @@
|
||||
#include <cstdlib>
|
||||
#include <cstdio>
|
||||
#include <math.h>
|
||||
#include <algorithm>
|
||||
#include <map>
|
||||
|
||||
#include "opencv2/opencv.hpp"
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <camera_info_manager/camera_info_manager.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <pcl_ros/point_cloud.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <velodyne_pointcloud/point_types.h>
|
||||
#include <pcl/common/eigen.h>
|
||||
#include <pcl/common/transforms.h>
|
||||
#include <pcl/filters/passthrough.h>
|
||||
|
||||
#include "lidar_camera_calibration/Corners.h"
|
||||
#include "lidar_camera_calibration/PreprocessUtils.h"
|
||||
#include "lidar_camera_calibration/Find_RT.h"
|
||||
|
||||
#include "lidar_camera_calibration/marker_6dof.h"
|
||||
|
||||
using namespace cv;
|
||||
using namespace std;
|
||||
using namespace ros;
|
||||
using namespace message_filters;
|
||||
using namespace pcl;
|
||||
|
||||
|
||||
string CAMERA_INFO_TOPIC;
|
||||
string VELODYNE_TOPIC;
|
||||
|
||||
|
||||
Mat projection_matrix;
|
||||
|
||||
pcl::PointCloud<myPointXYZRID> point_cloud;
|
||||
Hesai::PointCloud point_cloud_hesai;
|
||||
|
||||
Eigen::Quaterniond qlidarToCamera;
|
||||
Eigen::Matrix3d lidarToCamera;
|
||||
|
||||
|
||||
void callback_noCam(const sensor_msgs::PointCloud2ConstPtr& msg_pc,
|
||||
const lidar_camera_calibration::marker_6dof::ConstPtr& msg_rt)
|
||||
{
|
||||
ROS_INFO_STREAM("Velodyne scan received at " << msg_pc->header.stamp.toSec());
|
||||
ROS_INFO_STREAM("marker_6dof received at " << msg_rt->header.stamp.toSec());
|
||||
|
||||
// Loading Velodyne point cloud_sub
|
||||
if (config.lidar_type == 0) // velodyne lidar
|
||||
{
|
||||
fromROSMsg(*msg_pc, point_cloud);
|
||||
}
|
||||
else if (config.lidar_type == 1) //hesai lidar
|
||||
{
|
||||
fromROSMsg(*msg_pc, point_cloud_hesai);
|
||||
point_cloud = *(toMyPointXYZRID(point_cloud_hesai));
|
||||
}
|
||||
|
||||
point_cloud = transform(point_cloud, config.initialTra[0], config.initialTra[1], config.initialTra[2], config.initialRot[0], config.initialRot[1], config.initialRot[2]);
|
||||
|
||||
//Rotation matrix to transform lidar point cloud to camera's frame
|
||||
|
||||
qlidarToCamera = Eigen::AngleAxisd(config.initialRot[2], Eigen::Vector3d::UnitZ())
|
||||
*Eigen::AngleAxisd(config.initialRot[1], Eigen::Vector3d::UnitY())
|
||||
*Eigen::AngleAxisd(config.initialRot[0], Eigen::Vector3d::UnitX());
|
||||
|
||||
lidarToCamera = qlidarToCamera.matrix();
|
||||
|
||||
std:: cout << "\n\nInitial Rot" << lidarToCamera << "\n";
|
||||
point_cloud = intensityByRangeDiff(point_cloud, config);
|
||||
// x := x, y := -z, z := y
|
||||
|
||||
//pcl::io::savePCDFileASCII ("/home/vishnu/PCDs/msg_point_cloud.pcd", pc);
|
||||
|
||||
|
||||
cv::Mat temp_mat(config.s, CV_8UC3);
|
||||
pcl::PointCloud<pcl::PointXYZ> retval = *(toPointsXYZ(point_cloud));
|
||||
|
||||
std::vector<float> marker_info;
|
||||
|
||||
for(std::vector<float>::const_iterator it = msg_rt->dof.data.begin(); it != msg_rt->dof.data.end(); ++it)
|
||||
{
|
||||
marker_info.push_back(*it);
|
||||
std::cout << *it << " ";
|
||||
}
|
||||
std::cout << "\n";
|
||||
|
||||
bool no_error = getCorners(temp_mat, retval, config.P, config.num_of_markers, config.MAX_ITERS);
|
||||
if(no_error){
|
||||
find_transformation(marker_info, config.num_of_markers, config.MAX_ITERS, lidarToCamera);
|
||||
}
|
||||
//ros::shutdown();
|
||||
}
|
||||
|
||||
void callback(const sensor_msgs::CameraInfoConstPtr& msg_info,
|
||||
const sensor_msgs::PointCloud2ConstPtr& msg_pc,
|
||||
const lidar_camera_calibration::marker_6dof::ConstPtr& msg_rt)
|
||||
{
|
||||
|
||||
ROS_INFO_STREAM("Camera info received at " << msg_info->header.stamp.toSec());
|
||||
ROS_INFO_STREAM("Velodyne scan received at " << msg_pc->header.stamp.toSec());
|
||||
ROS_INFO_STREAM("marker_6dof received at " << msg_rt->header.stamp.toSec());
|
||||
|
||||
float p[12];
|
||||
float *pp = p;
|
||||
for (boost::array<double, 12ul>::const_iterator i = msg_info->P.begin(); i != msg_info->P.end(); i++)
|
||||
{
|
||||
*pp = (float)(*i);
|
||||
pp++;
|
||||
}
|
||||
cv::Mat(3, 4, CV_32FC1, &p).copyTo(projection_matrix);
|
||||
|
||||
|
||||
|
||||
// Loading Velodyne point cloud_sub
|
||||
if (config.lidar_type == 0) // velodyne lidar
|
||||
{
|
||||
fromROSMsg(*msg_pc, point_cloud);
|
||||
}
|
||||
else if (config.lidar_type == 1) //hesai lidar
|
||||
{
|
||||
fromROSMsg(*msg_pc, point_cloud_hesai);
|
||||
point_cloud = *(toMyPointXYZRID(point_cloud_hesai));
|
||||
}
|
||||
|
||||
point_cloud = transform(point_cloud, config.initialTra[0], config.initialTra[1], config.initialTra[2], config.initialRot[0], config.initialRot[1], config.initialRot[2]);
|
||||
|
||||
//Rotation matrix to transform lidar point cloud to camera's frame
|
||||
|
||||
qlidarToCamera = Eigen::AngleAxisd(config.initialRot[2], Eigen::Vector3d::UnitZ())
|
||||
*Eigen::AngleAxisd(config.initialRot[1], Eigen::Vector3d::UnitY())
|
||||
*Eigen::AngleAxisd(config.initialRot[0], Eigen::Vector3d::UnitX());
|
||||
|
||||
lidarToCamera = qlidarToCamera.matrix();
|
||||
|
||||
point_cloud = intensityByRangeDiff(point_cloud, config);
|
||||
// x := x, y := -z, z := y
|
||||
|
||||
//pcl::io::savePCDFileASCII ("/home/vishnu/PCDs/msg_point_cloud.pcd", pc);
|
||||
|
||||
|
||||
cv::Mat temp_mat(config.s, CV_8UC3);
|
||||
pcl::PointCloud<pcl::PointXYZ> retval = *(toPointsXYZ(point_cloud));
|
||||
|
||||
std::vector<float> marker_info;
|
||||
|
||||
for(std::vector<float>::const_iterator it = msg_rt->dof.data.begin(); it != msg_rt->dof.data.end(); ++it)
|
||||
{
|
||||
marker_info.push_back(*it);
|
||||
std::cout << *it << " ";
|
||||
}
|
||||
std::cout << "\n";
|
||||
|
||||
getCorners(temp_mat, retval, projection_matrix, config.num_of_markers, config.MAX_ITERS);
|
||||
find_transformation(marker_info, config.num_of_markers, config.MAX_ITERS, lidarToCamera);
|
||||
//ros::shutdown();
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
readConfig();
|
||||
ros::init(argc, argv, "find_transform");
|
||||
|
||||
ros::NodeHandle n;
|
||||
|
||||
if(config.useCameraInfo)
|
||||
{
|
||||
ROS_INFO_STREAM("Reading CameraInfo from topic");
|
||||
n.getParam("/lidar_camera_calibration/camera_info_topic", CAMERA_INFO_TOPIC);
|
||||
n.getParam("/lidar_camera_calibration/velodyne_topic", VELODYNE_TOPIC);
|
||||
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub(n, CAMERA_INFO_TOPIC, 1);
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub(n, VELODYNE_TOPIC, 1);
|
||||
message_filters::Subscriber<lidar_camera_calibration::marker_6dof> rt_sub(n, "lidar_camera_calibration_rt", 1);
|
||||
|
||||
std::cout << "done1\n";
|
||||
|
||||
typedef sync_policies::ApproximateTime<sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, lidar_camera_calibration::marker_6dof> MySyncPolicy;
|
||||
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), info_sub, cloud_sub, rt_sub);
|
||||
sync.registerCallback(boost::bind(&callback, _1, _2, _3));
|
||||
|
||||
ros::spin();
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_INFO_STREAM("Reading CameraInfo from configuration file");
|
||||
n.getParam("/lidar_camera_calibration/velodyne_topic", VELODYNE_TOPIC);
|
||||
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub(n, VELODYNE_TOPIC, 1);
|
||||
message_filters::Subscriber<lidar_camera_calibration::marker_6dof> rt_sub(n, "lidar_camera_calibration_rt", 1);
|
||||
|
||||
typedef sync_policies::ApproximateTime<sensor_msgs::PointCloud2, lidar_camera_calibration::marker_6dof> MySyncPolicy;
|
||||
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), cloud_sub, rt_sub);
|
||||
sync.registerCallback(boost::bind(&callback_noCam, _1, _2));
|
||||
|
||||
ros::spin();
|
||||
}
|
||||
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
39
flow copy.py
Normal file
39
flow copy.py
Normal file
@ -0,0 +1,39 @@
|
||||
import open3d as o3d
|
||||
import numpy as np
|
||||
import cv2
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# 创建一些点云数据
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(np.random.rand(100, 3))
|
||||
|
||||
# 设置相机参数
|
||||
fx = 525.0 # 焦距
|
||||
fy = 525.0
|
||||
cx = 319.5 # 主点
|
||||
cy = 239.5
|
||||
depth_scale = 1000.0 # 深度缩放因子
|
||||
intrinsic = np.identity(3)
|
||||
intrinsic[0, 0] = fx
|
||||
intrinsic[0, 2] = cx
|
||||
intrinsic[1, 1] = fy
|
||||
intrinsic[1, 2] = cy
|
||||
|
||||
# 将点云投影到图像平面
|
||||
pcd.transform(intrinsic)
|
||||
|
||||
# 转换为OpenCV的图像格式
|
||||
points = np.asarray(pcd.points)
|
||||
depth = points[:, 2]
|
||||
depth[depth < 0] = 0 # 设置负的深度值为0
|
||||
depth /= depth_scale # 归一化深度
|
||||
h, w = int(cy + 1), int(cx + 1)
|
||||
image = np.zeros((h, w, 3), dtype=np.uint8)
|
||||
for i in range(points.shape[0]):
|
||||
x, y, z = points[i]
|
||||
if 0 <= int(y) < h and 0 <= int(x) < w:
|
||||
image[int(y), int(x)] = (int(255 * (1 - z / depth[i])), int(255 * (1 - z / depth[i])), int(255 * (1 - z / depth[i])))
|
||||
|
||||
# 显示结果
|
||||
plt.imshow(cv2.cvtColor(image, cv2.COLOR_BGR2RGB))
|
||||
plt.show()
|
||||
354
flow_edge.py
Normal file
354
flow_edge.py
Normal file
@ -0,0 +1,354 @@
|
||||
# -- coding: utf-8 --
|
||||
import numpy as np
|
||||
import cv2
|
||||
import math
|
||||
|
||||
|
||||
def _sobel(image):
|
||||
'''
|
||||
_sobel
|
||||
'''
|
||||
if image.ndim > 2:
|
||||
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
|
||||
# todo 增加几个参数 http://blog.csdn.net/sunny2038/article/details/9170013
|
||||
_sobelx = cv2.Sobel(image, cv2.CV_64F, 1, 0, ksize=1)
|
||||
_sobely = cv2.Sobel(image, cv2.CV_64F, 0, 1, ksize=1)
|
||||
|
||||
_sobelx = np.uint8(np.absolute(_sobelx))
|
||||
_sobely = np.uint8(np.absolute(_sobely))
|
||||
_sobelcombine = cv2.bitwise_or(_sobelx,_sobely)
|
||||
return _sobelcombine
|
||||
|
||||
# ##################################################################
|
||||
|
||||
# # 展平
|
||||
# img_flat = _im_gray.reshape((_im_gray.shape[0] * _im_gray.shape[1], 1))
|
||||
|
||||
|
||||
# img_flat = np.float32(img_flat)
|
||||
|
||||
# # 迭代参数
|
||||
# criteria = (cv2.TERM_CRITERIA_EPS + cv2.TermCriteria_MAX_ITER, 20, 0.0)
|
||||
# flags = cv2.KMEANS_RANDOM_CENTERS
|
||||
|
||||
# # 进行聚类
|
||||
# compactness, labels, centers = cv2.kmeans(img_flat, 20, None, criteria, 10, flags)
|
||||
|
||||
# segmented_img = labels.reshape(_im_gray.shape)
|
||||
# _im_gray = np.uint8(segmented_img)
|
||||
# _im_gray = cv2.equalizeHist(_im_gray)
|
||||
|
||||
# cv2.imshow("_im2", im)
|
||||
# cv2.waitKey(0)
|
||||
# ##################################################################
|
||||
|
||||
def _findContours(image):
|
||||
'''
|
||||
_findContours
|
||||
http://blog.csdn.net/mokeding/article/details/20153325
|
||||
'''
|
||||
contours, _ = cv2.findContours(image.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
|
||||
return sorted(contours, key=cv2.contourArea, reverse=True)
|
||||
|
||||
def _drawContours(image, contours, cidx=-1):
|
||||
'''
|
||||
_drawContours
|
||||
'''
|
||||
image_contour = cv2.drawContours(image, contours, -1, (255, 255, 255), 1)
|
||||
return image_contour
|
||||
|
||||
import time
|
||||
class CropLayer(object):
|
||||
def __init__(self, params, blobs):
|
||||
self.xstart = 0
|
||||
self.xend = 0
|
||||
self.ystart = 0
|
||||
self.yend = 0
|
||||
# Our layer receives two inputs. We need to crop the first input blob
|
||||
# to match a shape of the second one (keeping batch size and number of channels)
|
||||
def getMemoryShapes(self, inputs):
|
||||
inputShape, targetShape = inputs[0], inputs[1]
|
||||
batchSize, numChannels = inputShape[0], inputShape[1]
|
||||
height, width = targetShape[2], targetShape[3]
|
||||
|
||||
self.ystart = int((inputShape[2] - targetShape[2]) / 2)
|
||||
self.xstart = int((inputShape[3] - targetShape[3]) / 2)
|
||||
self.yend = self.ystart + height
|
||||
self.xend = self.xstart + width
|
||||
|
||||
return [[batchSize, numChannels, height, width]]
|
||||
|
||||
def forward(self, inputs):
|
||||
return [inputs[0][:,:,self.ystart:self.yend,self.xstart:self.xend]]
|
||||
|
||||
class Arguments:
|
||||
def __init__(self):
|
||||
self.description = ""
|
||||
self.input = "./images/x.png"
|
||||
self.prototxt = "./deploy.prototxt"
|
||||
self.caffemodel = "./hed_pretrained_bsds.caffemodel"
|
||||
self.width = 360
|
||||
self.height = 115
|
||||
self.savefile = "./1.jpg"
|
||||
|
||||
def dnn(im):
|
||||
args = Arguments()
|
||||
args.width = im.shape[1]
|
||||
args.height = im.shape[0]
|
||||
args.input = im
|
||||
# Load the model.
|
||||
net = cv2.dnn.readNetFromCaffe(args.prototxt, args.caffemodel)
|
||||
# 设置网络运行参数
|
||||
net.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV)
|
||||
net.setPreferableTarget(cv2.dnn.DNN_TARGET_CPU) # 或者GPU
|
||||
# net.setPreferableTarget(cv2.dnn.DNN_TARGET_OPENCL)
|
||||
cv2.dnn_registerLayer('Crop', CropLayer)
|
||||
kWinName = 'Holistically-Nested_Edge_Detection'
|
||||
mean_value = cv2.mean(im)
|
||||
inp = cv2.dnn.blobFromImage(im, scalefactor=1.5, size=(args.width, args.height),
|
||||
mean=mean_value,
|
||||
swapRB=True, crop=True)
|
||||
net.setInput(inp)
|
||||
# edges = cv2.Canny(frame,args.width,args.height)
|
||||
# edges = cv2.Canny(frame,frame.shape[1],frame.shape[0])
|
||||
out = net.forward()
|
||||
out = out[0, 0]
|
||||
_im_gray = cv2.resize(out, (im.shape[1], im.shape[0]))
|
||||
_im_gray = 255 * _im_gray
|
||||
# print(out)
|
||||
_im_gray = _im_gray.astype(np.uint8)
|
||||
_im_gray = cv2.resize(_im_gray, (640, 480))
|
||||
cv2.imshow('Input', _im_gray)
|
||||
cv2.waitKey(0)
|
||||
|
||||
def Scharr(image):
|
||||
'''
|
||||
_sobel
|
||||
'''
|
||||
if image.ndim > 2:
|
||||
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
|
||||
# todo 增加几个参数 http://blog.csdn.net/sunny2038/article/details/9170013
|
||||
_scharrx = cv2.Scharr(image, cv2.CV_8U, dx=1, dy=0, scale=1, delta=0, borderType=cv2.BORDER_DEFAULT)
|
||||
_scharry = cv2.Scharr(image, cv2.CV_8U, dx=0, dy=1, scale=1, delta=0, borderType=cv2.BORDER_DEFAULT)
|
||||
|
||||
_scharrx = np.uint8(np.absolute(_scharrx))
|
||||
_scharry = np.uint8(np.absolute(_scharry))
|
||||
_sobelcombine = cv2.bitwise_or(_scharrx,_scharry)
|
||||
|
||||
return _sobelcombine
|
||||
|
||||
def cal_conners_edges_sort(im, thresh_hold = 80, minLineLength = 400):
|
||||
|
||||
_im_gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
|
||||
# for test
|
||||
_im_gray = cv2.GaussianBlur(_im_gray, (5, 5), 0)
|
||||
_im_edge_sobel = _sobel(_im_gray)
|
||||
# _im_edge_sobel = Scharr(_im_gray)
|
||||
|
||||
_, _im_thresh = cv2.threshold(_im_edge_sobel, 5, 255, cv2.THRESH_BINARY)
|
||||
|
||||
cnts = _findContours(_im_thresh)
|
||||
for contour in cnts:
|
||||
if cv2.contourArea(contour) > 500:
|
||||
# 计算轮廓的边界框
|
||||
x, y, w, h = cv2.boundingRect(contour)
|
||||
# 在原图上绘制矩形
|
||||
cv2.rectangle(_im, (x, y), (x + w, y + h), (0, 255, 0), 2)
|
||||
cv2.imshow("_im2", _im_thresh)
|
||||
cv2.waitKey(0)
|
||||
|
||||
_lines = cv2.HoughLinesP(_im_thresh, 1, np.pi / 180, thresh_hold, minLineLength=minLineLength, maxLineGap=120)
|
||||
|
||||
# for test
|
||||
for _line in _lines:
|
||||
x1,y1,x2,y2 = _line[0]
|
||||
cv2.line(_im, (x1,y1), (x2,y2), (0,255,0), 1)
|
||||
# for test
|
||||
cv2.imshow("_im", _im)
|
||||
cv2.imshow("_im2", _im_edge_sobel)
|
||||
cv2.waitKey(0)
|
||||
|
||||
# print(len(_lines))
|
||||
_line4 = []
|
||||
for _line in _lines:
|
||||
x1,y1,x2,y2 = _line[0]
|
||||
if x2 == x1: x2 += 0.001
|
||||
theta = np.arctan((y2 - y1) / (x2 - x1)) * (180 / math.pi)
|
||||
x2 = int(x2)
|
||||
r = math.sqrt(pow((y2 - y1), 2) + pow((x2 - x1), 2))
|
||||
# 计算直线的斜率(如果 1 ≠ 2)
|
||||
if x2 == x1: x2 += 0.001 # 如果x2 == x1，会出现分母为0
|
||||
m = (y2 - y1) / (x2 - x1)
|
||||
x2 = int(x2)
|
||||
A, B, C = m, -1, y1 - m * x1
|
||||
brepeat = False
|
||||
for i in range(len(_line4)):
|
||||
if abs(abs(theta) - abs(_line4[i][4])) < 30: # 20°
|
||||
cx, cy = (_line4[i][2] + _line4[i][0]) / 2, (_line4[i][3] + _line4[i][1]) / 2
|
||||
d = abs(A * cx + B * cy + C) / math.sqrt(pow(A, 2) + pow(B, 2))
|
||||
r_max = max(_line4[i][5], r)
|
||||
if d < r_max / 3:
|
||||
brepeat = True
|
||||
if (r > _line4[i][5]) :
|
||||
_line4[i] = [x1,y1,x2,y2, theta, r]
|
||||
if not brepeat :
|
||||
_line4.append([x1,y1,x2,y2, theta, r])
|
||||
# print(x1,y1,x2,y2, theta, r)
|
||||
|
||||
# # for test
|
||||
for x1,y1,x2,y2, theta, r in _line4:
|
||||
cv2.line(_im, (x1,y1), (x2,y2), (0,255,0), 10)
|
||||
print(x1,y1,x2,y2, theta, r)
|
||||
_im_samll = cv2.resize(_im, (500, 500))
|
||||
# for test
|
||||
cv2.imshow("_im", _im_samll)
|
||||
cv2.waitKey(0)
|
||||
|
||||
# 4条边排序
|
||||
# assert(len(_line4) == 4)
|
||||
_line4_sort = []
|
||||
Ax, Bx, Cx = 1, 0, 0
|
||||
Ay, By, Cy = 0, 1, 0
|
||||
min_dx, max_dx, min_dy, max_dy = 10000, 0, 10000, 0
|
||||
min_dx_idx, max_dx_id, min_dy_id, max_dy_id = -1, -1, -1, -1
|
||||
for i in range(len(_line4)):
|
||||
cx, cy = (_line4[i][2] + _line4[i][0]) / 2, (_line4[i][3] + _line4[i][1]) / 2
|
||||
dx = abs(Ay * cx + By * cy + Cy) / math.sqrt(pow(Ay, 2) + pow(By, 2))
|
||||
dy = abs(Ax * cx + Bx * cy + Cx) / math.sqrt(pow(Ax, 2) + pow(Bx, 2))
|
||||
if dx < min_dx:
|
||||
min_dx = dx
|
||||
min_dx_idx = i
|
||||
if dx > max_dx:
|
||||
max_dx = dx
|
||||
max_dx_id = i
|
||||
if dy < min_dy:
|
||||
min_dy = dy
|
||||
min_dy_id = i
|
||||
if dy > max_dy:
|
||||
max_dy = dy
|
||||
max_dy_id = i
|
||||
_line4_sort = _line4[min_dx_idx], _line4[max_dy_id], _line4[max_dx_id], _line4[min_dy_id]
|
||||
print(_line4_sort)
|
||||
|
||||
# # for test
|
||||
for x1,y1,x2,y2, theta, r in _line4_sort:
|
||||
cv2.line(_im, (x1,y1), (x2,y2), (0,0,255), 10)
|
||||
print(x1,y1,x2,y2, theta, r)
|
||||
_im_samll = cv2.resize(_im, (500, 500))
|
||||
# for test
|
||||
cv2.imshow("_im", _im_samll)
|
||||
cv2.waitKey(0)
|
||||
|
||||
# 找四个交点
|
||||
_conners4_sort = [0,0,0,0]
|
||||
for i in range(len(_line4_sort)):
|
||||
x1, y1, x2, y2, _, _ = _line4_sort[i]
|
||||
x1_next, y1_next, x2_next, y2_next, _, _ = _line4_sort[(i + 1) % 4]
|
||||
|
||||
# 检查是否为垂直线
|
||||
# if x2 - x1 == 0:
|
||||
# A, B, C = 1, 0, -x1 # 垂直线
|
||||
# else:
|
||||
if x2 == x1: x2 += 0.00001 # 如果x2 == x1，会出现分母为0
|
||||
m = (y2 - y1) / (x2 - x1)
|
||||
x2 = int(x2)
|
||||
A, B, C = m, -1, y1 - m * x1
|
||||
|
||||
# if x2_next - x1_next == 0:
|
||||
# A_next, B_next, C_next = 1, 0, -x1_next # 垂直线
|
||||
# else:
|
||||
if x2_next == x1_next: x2_next += 0.001 # 如果x2 == x1，会出现分母为0
|
||||
m_next = (y2_next - y1_next) / (x2_next - x1_next)
|
||||
x2_next = int(x2_next)
|
||||
A_next, B_next, C_next = m_next, -1, y1_next - m_next * x1_next
|
||||
|
||||
# 检查是否平行
|
||||
if A * B_next == A_next * B:
|
||||
continue # 跳过平行的线段
|
||||
|
||||
# 计算交点
|
||||
x_p = (B_next * C - B * C_next) / (A_next * B - A * B_next)
|
||||
y_p = (A * x_p + C) / - B
|
||||
|
||||
# 确保交点在图像范围内
|
||||
if 0 <= int(x_p) < _im.shape[1] and 0 <= int(y_p) < _im.shape[0]:
|
||||
_conners4_sort[(i + 1) % 4] = [int(x_p), int(y_p)]
|
||||
|
||||
assert(len(_conners4_sort) == 4)
|
||||
|
||||
return _conners4_sort, _line4_sort
|
||||
|
||||
|
||||
def cal_sub_conners(im, conners4_sort, line4_sort, sub_pixels=100):
|
||||
assert(len(conners4_sort)==4)
|
||||
|
||||
for i in range(4):
|
||||
cx, cy = conners4_sort[i]
|
||||
x = int(cx - sub_pixels / 2)
|
||||
if x < 0: x = 0
|
||||
y = int(cy - sub_pixels / 2)
|
||||
if y < 0: y = 0
|
||||
w = sub_pixels
|
||||
h = sub_pixels
|
||||
|
||||
roi = im[y:y+h, x:x+w].copy()
|
||||
roi_gray = cv2.cvtColor(roi, cv2.COLOR_RGB2GRAY)
|
||||
roi_gray = cv2.Canny(roi_gray, threshold1=100, threshold2=200)
|
||||
_tmp_corners = cv2.goodFeaturesToTrack(roi_gray, maxCorners=100, qualityLevel=0.001, minDistance=2)
|
||||
|
||||
roi_cx = cx - x
|
||||
roi_cy = cy - y
|
||||
min = 10000
|
||||
tar_conner = []
|
||||
|
||||
for conner in _tmp_corners:
|
||||
conner = conner.tolist()[0]
|
||||
d = math.sqrt(pow((conner[0] - roi_cx), 2) + pow((conner[1] - roi_cy), 2))
|
||||
if d < min :
|
||||
min = d
|
||||
tar_conner = [conner[0], conner[1]]
|
||||
cv2.circle(_im, (int(conner[0]+x), int(conner[1]+y)), 1, (255,0,0), -1)
|
||||
cv2.circle(roi, (int(roi_cx), int(roi_cy)), 2, (0,0,255), -1)
|
||||
cv2.circle(roi, (int(tar_conner[0]), int(tar_conner[1])), 2, (255,0,0), -1)
|
||||
cv2.circle(_im, (int(roi_cx+x), int(roi_cy+y)), 2, (0,0,255), -1)
|
||||
cv2.circle(_im, (int(tar_conner[0]+x), int(tar_conner[1]+y)), 2, (255,0,0), -1)
|
||||
# corners = np.int0(corners)
|
||||
|
||||
# for test
|
||||
cv2.imshow("roi", roi)
|
||||
cv2.waitKey(0)
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
_im = cv2.imread("./3_roi_image.png")
|
||||
|
||||
edge = np.sqrt(pow((0.323 - 0.32222), 2)
|
||||
+ pow((1.32- 14.2), 2)
|
||||
+ pow((41 - 32.1), 2))
|
||||
cv2.putText(_im, str(edge)[:6], (int((23+23)/2), int((23+32)/2)), \
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
|
||||
|
||||
|
||||
_gray = cv2.cvtColor(_im, cv2.COLOR_BGR2GRAY)
|
||||
# 测试自适应阈值分割
|
||||
# thresh = cv2.adaptiveThreshold(_gray, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 11, 2)
|
||||
# t, result_img = cv2.threshold(_gray, 5, 255, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
|
||||
# print(t)
|
||||
# cv2.imshow("thresh", result_img)
|
||||
# cv2.waitKey(0)
|
||||
|
||||
|
||||
_conners4_sort, _line4_sort = cal_conners_edges_sort(_im)
|
||||
|
||||
# cal_sub_conners(_im, _conners4_sort, _line4_sort)
|
||||
|
||||
for i in range(4):
|
||||
x1, y1, x2, y2, _, _ = _line4_sort[i]
|
||||
x_p, y_p = _conners4_sort[i]
|
||||
# # for test
|
||||
cv2.circle(_im, (int(x_p), int(y_p)), 2, (0,0,255), -1)
|
||||
cv2.line(_im, (x1,y1), (x2,y2), (0,255,0), 1)
|
||||
|
||||
cv2.imwrite("./2_roi_imagerest.png", _im)
|
||||
cv2.imshow("im_edge", _im)
|
||||
cv2.waitKey(0)
|
||||
223
flow_homo.py
Normal file
223
flow_homo.py
Normal file
@ -0,0 +1,223 @@
|
||||
# -- coding: utf-8 --
|
||||
import numpy as np
|
||||
import cv2
|
||||
import math
|
||||
import open3d as o3d
|
||||
|
||||
|
||||
class rect_roi:
|
||||
def __init__(self, x:int, y:int, w:int, h:int):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.w = w
|
||||
self.h = h
|
||||
|
||||
|
||||
class obj_info:
|
||||
def __init__(self, name:str, id:str, points_2d_to_3d:cv2.Mat, bim=None):
|
||||
self.name = name
|
||||
self.id = id
|
||||
self.rect_points_2d_to_3d = points_2d_to_3d
|
||||
self.bim = bim
|
||||
self.edges = []
|
||||
self.orgin_pt = [0.0, 0.0]
|
||||
self.centor_pt = [0.0, 0.0]
|
||||
|
||||
|
||||
def _api_cal_homography(_3d_points, _2d_points):
|
||||
ransacReprojThreshold = 5.0 # Homography: 该值越小，精度越高，对时间没有影像
|
||||
# 查找单应矩阵
|
||||
# 修改第三个参数 1.0 -> 5.0
|
||||
# 利用基于RANSAC的鲁棒算法选择最优的四组配对点，再计算转换矩阵H(3*3)并返回,以便于反向投影错误率达到最小
|
||||
# ransacReprojThreshold 设置为 1.0，精度最高
|
||||
# H, mask = cv2.findHomography(np.array(src_points), np.array(dst_points), cv2.RANSAC, ransacReprojThreshold, maxIters=1500, confidence=0.998) ## 修改 1.0 -> 5.0
|
||||
pts_hom = []
|
||||
for i in range(len(_2d_points)):
|
||||
pt = _2d_points[i]
|
||||
x,y = pt[0], pt[1]
|
||||
pts_hom.append([x*1.0, y*1.0, 1.0])
|
||||
H, _ = cv2.findHomography(np.array(_3d_points), np.array(pts_hom), cv2.RANSAC, ransacReprojThreshold) ## 修改 1.0 -> 5.0
|
||||
print(H)
|
||||
return H
|
||||
|
||||
|
||||
# 暂时不需要
|
||||
def _api_get_fake_roi_data():
|
||||
return [[]]
|
||||
|
||||
|
||||
def _api_get_obj_2d_roi(im, rois):
|
||||
_roi_im_list = []
|
||||
for i in range(len(rois)):
|
||||
roi = rois[i]
|
||||
_roi_im = im[roi.y:roi.y+roi.h, roi.x:roi.x+roi.w]
|
||||
_roi_im_list.append(_roi_im)
|
||||
return _roi_im_list
|
||||
|
||||
|
||||
def __get_small_conners(conners, offset):
|
||||
# pts = np.array([[conners[0][0] - offset, conners[0][1] - offset],
|
||||
# [conners[1][0] - offset, conners[1][1] - offset],
|
||||
# [conners[2][0] - offset, conners[2][1] - offset],
|
||||
# [conners[3][0] - offset, conners[3][1] - offset]],
|
||||
# np.int32)
|
||||
pts = [[conners[0][0] + offset, conners[0][1]],
|
||||
[conners[1][0] - offset, conners[1][1] + offset],
|
||||
[conners[2][0] - offset, conners[2][1]],
|
||||
[conners[3][0], conners[3][1] - offset]]
|
||||
return pts
|
||||
# cv2.polylines(img, [pts], isClosed=True, color=(0, 255, 0), thickness=2)
|
||||
|
||||
|
||||
def _api_get_sub_rois_with_conners(conners):
|
||||
_conners = []
|
||||
_conners.append(__get_small_conners(conners, 200))
|
||||
return _conners
|
||||
|
||||
|
||||
def _api_get_obj_2d_to_3d_pts(im_2d_to_3d:cv2.Mat, sub_rois_in_conners):
|
||||
in_rect_2d_to_3d_pts = []
|
||||
in_rect_2d_pts = []
|
||||
in_rect_3d_pts = []
|
||||
|
||||
for i in range(len(sub_rois_in_conners)):
|
||||
_sub_rois = sub_rois_in_conners[i]
|
||||
_sub_rois_mask = np.zeros_like(im_2d_to_3d)
|
||||
|
||||
# for test
|
||||
# normalized_image = np.uint8(_sub_rois_mask)
|
||||
# _sub_rois_mask = cv2.normalize(normalized_image, None, 0, 255, cv2.NORM_MINMAX)
|
||||
|
||||
_sub_rois_mask = cv2.fillPoly(_sub_rois_mask, [np.array(_sub_rois)], color=(255, 255, 255))
|
||||
im_2d_to_3d_roi = cv2.bitwise_and(im_2d_to_3d, _sub_rois_mask)
|
||||
# im_2d_to_3d_roi = cv2.resize(im_2d_to_3d_roi, (int(934*1.5), int(700*1.5)), interpolation=cv2.INTER_LINEAR)
|
||||
# cv2.imshow("1", im_2d_to_3d_roi)
|
||||
# cv2.waitKey(0)
|
||||
|
||||
normalized_image = np.uint8(im_2d_to_3d_roi)
|
||||
normalized_image = cv2.cvtColor(normalized_image, cv2.COLOR_BGR2GRAY)
|
||||
_2d_pts = cv2.findNonZero(normalized_image)
|
||||
|
||||
for i in range(len(_2d_pts)):
|
||||
_2d_pt = _2d_pts[i][0].tolist()
|
||||
if i % 20 == 0:
|
||||
in_rect_2d_pts.append(_2d_pt)
|
||||
in_rect_3d_pts.append([im_2d_to_3d[_2d_pt[1],_2d_pt[0]][0],
|
||||
im_2d_to_3d[_2d_pt[1],_2d_pt[0]][1],
|
||||
im_2d_to_3d[_2d_pt[1],_2d_pt[0]][2]])
|
||||
return in_rect_2d_to_3d_pts, in_rect_2d_pts, in_rect_3d_pts
|
||||
|
||||
|
||||
# _conners：返回预埋件角点在原图中的坐标（非在roi图中的坐标）
|
||||
def _api_get_edges_and_conners(roi_im, roi_rect, fake=True):
|
||||
_edges = []
|
||||
_conners = []
|
||||
rst = "ok" # 如果识别改区域不是预埋件区域，则返回错误
|
||||
if fake:
|
||||
# 人工定好4个角点
|
||||
_conners = [[465,436],[615,260],[783,405],[630,581]]
|
||||
for i in range(4):
|
||||
_conners[i] = [int(_conners[i][0]*9344/934/1.5), int(_conners[i][1]*7000/700/1.5)]
|
||||
return rst, _edges, _conners
|
||||
|
||||
|
||||
# 在未来使用roi_im, roi_rect进行优化
|
||||
# def _api_cal_3d_edge_scale(roi_im, roi_rect, conners, H):
|
||||
def _api_cal_3d_edge_scale(conners, H, _2pts, _3pts):
|
||||
|
||||
_edges = []
|
||||
|
||||
_conners_3d = []
|
||||
|
||||
if len(conners) != 4:
|
||||
assert("_cal_scale: len(conners) is not 4")
|
||||
|
||||
# 计算点云的拟合平面
|
||||
_3pts_cloud = o3d.geometry.PointCloud()
|
||||
_3pts_cloud.points = o3d.utility.Vector3dVector(_3pts)
|
||||
[A,B,C,D], inliers = _3pts_cloud.segment_plane(distance_threshold=0.01, ransac_n=3, num_iterations=1500)
|
||||
# [a,b,c,d]= plane model
|
||||
|
||||
H_inv = np.linalg.inv(H)
|
||||
for i in range(4):
|
||||
conner = conners[i]
|
||||
x, y = conner[0], conner[1]
|
||||
pts_hom = np.array([x, y, 1])
|
||||
# pts_hom = cv2.convertPointsToHomogeneous(pts)
|
||||
pts_3d = np.dot(H_inv, pts_hom.T)
|
||||
pts_3d = pts_3d/pts_3d[2]
|
||||
|
||||
# 求线与平面的交点
|
||||
t = -D / (A * pts_3d[0] + B * pts_3d[1] + C * pts_3d[2])
|
||||
pts_3d = np.array([t * pts_3d[0], t * pts_3d[1], t * pts_3d[2]])
|
||||
|
||||
_conners_3d.append(list(pts_3d))
|
||||
|
||||
for i in range(4):
|
||||
_conner_3d = _conners_3d[i]
|
||||
_conner_3d_next = _conners_3d[(i+1)%4]
|
||||
_edge = math.sqrt(pow((_conner_3d[0] - _conner_3d_next[0]), 2)
|
||||
+ pow((_conner_3d[1] - _conner_3d_next[1]), 2)
|
||||
+ pow((_conner_3d[2] - _conner_3d_next[2]), 2))
|
||||
_edges.append(_edge)
|
||||
return _edges
|
||||
|
||||
|
||||
# 主处理流程
|
||||
# for test: roi_rects = [[0,0,0,0]]
|
||||
def api_cal_info(im, im_2d_to_3d, roi_rects):
|
||||
|
||||
print("api_cal_info start!")
|
||||
|
||||
_obj_info_list = []
|
||||
|
||||
# for test，人工定义roi区域
|
||||
# roi_rects = _api_get_fake_roi_data()
|
||||
|
||||
# 图像剪切到一个个roi图像
|
||||
_roi_im_list = _api_get_obj_2d_roi(im, roi_rects)
|
||||
|
||||
# 遍历所有预埋件的子区域图像，这里可以分线程跑，提升效率
|
||||
for i in range(len(_roi_im_list)):
|
||||
|
||||
# 获取单个预埋件图像
|
||||
_roi_im = _roi_im_list[i]
|
||||
|
||||
# 在单个预埋件图像中计算区域内预埋件边缘和角点
|
||||
_rst, _, _2d_conners = _api_get_edges_and_conners(_roi_im, roi_rects[i])
|
||||
print("api_cal_info _2d_conners ==> ")
|
||||
print(_2d_conners)
|
||||
# assert(_rst != "ok")
|
||||
|
||||
# 计算预埋件内部用于检测 H矩阵的2d 3d区域
|
||||
# _sub_rois_in_conners：全局坐标系
|
||||
_sub_rois_in_conners = _api_get_sub_rois_with_conners(_2d_conners)
|
||||
|
||||
# 挑选合适的预埋件区域内2d和3d点映射，挑选算法需要设计和优化
|
||||
_, _2pts, _3pts = _api_get_obj_2d_to_3d_pts(im_2d_to_3d, _sub_rois_in_conners)
|
||||
|
||||
# 计算H矩阵
|
||||
H = _api_cal_homography(_3pts, _2pts)
|
||||
|
||||
# 通过角点和边缘计算尺寸
|
||||
# _2d_conners：为全局坐标，不是roi坐标
|
||||
# _edgs_scale = _api_cal_3d_edge_scale(_roi_im, roi_rects[i], _2d_conners, H)
|
||||
_edgs_scale = _api_cal_3d_edge_scale(_2d_conners, H, _2pts, _3pts)
|
||||
|
||||
print(_edgs_scale)
|
||||
|
||||
# _obj_info = obj_info()
|
||||
|
||||
# _obj_info_list.append(_obj_info)
|
||||
|
||||
print("api_cal_info end!")
|
||||
return _obj_info_list
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
pass
|
||||
|
||||
# _edge = math.sqrt(pow(-0.19919018 - 0.271936, 2)
|
||||
# + pow(-0.48743263 - -0.32997242, 2)
|
||||
# + pow(3.513779 - 3.527755, 2))
|
||||
# print(_edge)
|
||||
384
flow_main.py
Normal file
384
flow_main.py
Normal file
@ -0,0 +1,384 @@
|
||||
# -- coding: utf-8 --
|
||||
from ctypes import *
|
||||
import cv2 # 4.9.0
|
||||
print(cv2.__version__) # 4.9.0
|
||||
import numpy as np
|
||||
print(np.__version__) # 1.24.4
|
||||
# import pyc # https://github.com/PointCloudLibrary/pcl/releases/tag/pcl-1.8.1
|
||||
import open3d as o3d
|
||||
print(o3d.__version__) # 0.18.0
|
||||
import convert
|
||||
import copy
|
||||
import math
|
||||
import flow_homo
|
||||
import time
|
||||
|
||||
WIDTH = 9344
|
||||
HEIGH = 7000
|
||||
step = 40
|
||||
|
||||
# 相机内参矩阵，这里假设为简单的相机参数
|
||||
w = WIDTH
|
||||
h = HEIGH
|
||||
|
||||
fx = 11241.983
|
||||
fy = 11244.0599
|
||||
cx = 4553.03821
|
||||
cy = 3516.9118
|
||||
|
||||
k1 = -0.04052072
|
||||
k2 = 0.22211572
|
||||
p1 = 0.00042405
|
||||
p2 = -0.00367346
|
||||
k3 = -0.15639485
|
||||
|
||||
###################################################1：不带畸变，不带激光雷达补偿标定
|
||||
# R_mat_avg = np.asarray([
|
||||
# [0.999835, -0.0162967, 0.00799612],
|
||||
# [0.0164626, 0.999641, -0.0211427],
|
||||
# [-0.00764869, 0.0212709, 0.999744]])
|
||||
|
||||
# T_vector = np.array([0.115324, 0.0797254, 0.00324282])
|
||||
|
||||
# R_T_mat_avg = np.asarray([
|
||||
# [0.999835, -0.0162967, 0.00799612, 0.115324],
|
||||
# [0.0164626, 0.999641, -0.0211427, 0.0797254],
|
||||
# [-0.00764869, 0.0212709, 0.999744, 0.00324282],
|
||||
# [0.0, 0.0, 0.0, 1.0]])
|
||||
|
||||
# R_mat_fin = np.asarray([
|
||||
# [0.0079925, -0.999835, 0.0163003],
|
||||
# [-0.0211428, -0.0164661, -0.999641],
|
||||
# [0.999745, 0.007645, -0.0212709]])
|
||||
|
||||
# y,p,r = 1.93222, -1.5934, -0.345034
|
||||
|
||||
###################################################2：带畸变，带激光雷达补偿标定
|
||||
R_mat_avg = np.asarray([
|
||||
[0.999812, -0.0178348, 0.00765956],
|
||||
[0.0179972, 0.999603, -0.021687],
|
||||
[-0.00726974, 0.0218207, 0.999735]])
|
||||
|
||||
T_vector = np.array([0.109253, 0.0710213, 0.0175978])
|
||||
|
||||
R_T_mat_avg = np.asarray([
|
||||
[0.999812, -0.0178348, 0.00765956, 0.109253],
|
||||
[0.0179972, 0.999603, -0.021687, 0.0710213],
|
||||
[-0.00726974, 0.0218207, 0.999735, 0.0175978],
|
||||
[0.0, 0.0, 0.0, 1.0]])
|
||||
|
||||
R_mat_fin = np.asarray([
|
||||
[0.00765987, -0.999812, 0.0178345],
|
||||
[-0.021687, -0.0179969, -0.999603],
|
||||
[0.999735, 0.00727006, -0.0218207]])
|
||||
|
||||
y,p,r = 1.91032, -1.5938, -0.321605
|
||||
|
||||
# y,p,r = 1.57086, -1.57086, -0.0
|
||||
|
||||
###################################################3：不带畸变，不带激光雷达补偿标定
|
||||
# R_mat_avg = np.asarray([
|
||||
# [0.999849, -0.048776, 0.00900869],
|
||||
# [0.014993, 0.999805, -0.0128836],
|
||||
# [-0.00881525, 0.0130167, 0.999876]])
|
||||
|
||||
# T_vector = np.array([0.120222, 0.0442982, 0.00136219])
|
||||
|
||||
# R_T_mat_avg = np.asarray([
|
||||
# [0.999849, -0.048776, 0.00900869, 0.120222],
|
||||
# [0.014993, 0.999805, -0.0128836, 0.0442982],
|
||||
# [-0.00881525, 0.0130167, 0.999876, 0.00136219],
|
||||
# [0.0, 0.0, 0.0, 1.0]])
|
||||
|
||||
# R_mat_fin = np.asarray([
|
||||
# [0.009009, -0.999849, 0.0148772],
|
||||
# [0.0128836, -0.0149927, -0.999805],
|
||||
# [0.999876, 0.00881577, -0.0130167]])
|
||||
|
||||
# y,p,r = 2.18103, -1.58652, -0.595295
|
||||
|
||||
|
||||
# x,y,z
|
||||
def _eulerAnglesToRotationMatrix(theta) :
|
||||
R_x = np.array([[1, 0, 0 ],
|
||||
[0, math.cos(theta[0]), -math.sin(theta[0]) ],
|
||||
[0, math.sin(theta[0]), math.cos(theta[0]) ]
|
||||
])
|
||||
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ],
|
||||
[0, 1, 0 ],
|
||||
[-math.sin(theta[1]), 0, math.cos(theta[1]) ]
|
||||
])
|
||||
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
|
||||
[math.sin(theta[2]), math.cos(theta[2]), 0],
|
||||
[0, 0, 1]
|
||||
])
|
||||
R = np.dot(R_z, np.dot( R_y, R_x ))
|
||||
return R
|
||||
|
||||
def cal_project_matrix():
|
||||
# 假设camera_matrix和dist_coeffs是已知的摄像机内参和畸变参数
|
||||
camera_matrix = np.array([[fx, 0, cx],
|
||||
[0, fy, cy],
|
||||
[0, 0, 1]])
|
||||
|
||||
camera_matrix = np.array([[11550.4192, 0.00000000, 4672.28001],
|
||||
[0.00000000, 11546.8773, 3.49929103],
|
||||
[0.00000000e+00, 0.00000000, 1.00000000]])
|
||||
|
||||
|
||||
dist_coeffs = np.array([k1, k2, p1, p2, k3]) # 这里k1, k2, p1, p2, k3是畸变系数
|
||||
dist_coeffs = np.array([-0.0198130402, 0.0446498902, 0.000332909792, -0.000424586371, 0.371045970])
|
||||
# src_size是原始图像的大小
|
||||
src_size = (w, h)
|
||||
|
||||
# 计算新的摄像机矩阵
|
||||
new_camera_matrix = cv2.getOptimalNewCameraMatrix(camera_matrix, dist_coeffs, src_size, alpha=0)
|
||||
print(new_camera_matrix)
|
||||
# print(new_camera_matrix)
|
||||
return new_camera_matrix
|
||||
|
||||
|
||||
def cal_rectification_matrix():
|
||||
pass
|
||||
|
||||
|
||||
# 定义获取点云深度最大最小值
|
||||
def get_pcd_depth_range(pts_3d):
|
||||
# 初始化最小值和最大值为第一个点的x坐标
|
||||
min_x = 0
|
||||
max_x = 0
|
||||
|
||||
# 遍历所有点，更新最小值和最大值
|
||||
for point_3d in pts_3d:
|
||||
x = point_3d[0]
|
||||
if x < min_x:
|
||||
min_x = x
|
||||
elif x > max_x:
|
||||
max_x = x
|
||||
|
||||
return max_x, min_x
|
||||
|
||||
|
||||
# 定义函数根据深度获取颜色
|
||||
def get_color(cur_depth, max_depth, min_depth):
|
||||
scale = (max_depth - min_depth) / 10
|
||||
if cur_depth < min_depth:
|
||||
return (0, 0, 255) # 返回蓝色
|
||||
elif cur_depth < min_depth + scale:
|
||||
green = int((cur_depth - min_depth) / scale * 255)
|
||||
return (0, green, 255) # 返回蓝到黄的渐变色
|
||||
elif cur_depth < min_depth + scale * 2:
|
||||
red = int((cur_depth - min_depth - scale) / scale * 255)
|
||||
return (0, 255, 255 - red) # 返回黄到红的渐变色
|
||||
elif cur_depth < min_depth + scale * 4:
|
||||
blue = int((cur_depth - min_depth - scale * 2) / scale * 255)
|
||||
return (blue, 255, 0) # 返回红到绿的渐变色
|
||||
elif cur_depth < min_depth + scale * 7:
|
||||
green = int((cur_depth - min_depth - scale * 4) / scale * 255)
|
||||
return (255, 255 - green, 0) # 返回绿到黄的渐变色
|
||||
elif cur_depth < min_depth + scale * 10:
|
||||
blue = int((cur_depth - min_depth - scale * 7) / scale * 255)
|
||||
return (255, 0, blue) # 返回黄到蓝的渐变色
|
||||
else:
|
||||
return (255, 0, 255) # 返回紫色
|
||||
|
||||
|
||||
def undistort(distorted_img):
|
||||
camera_matrix = np.array([[fx, 0, cx],
|
||||
[0, fy, cy],
|
||||
[0, 0, 1]])
|
||||
dist_coeffs = np.array([k1, k2, p1, p2, k3]) # 畸变系数
|
||||
|
||||
# 矫正畸变
|
||||
undistorted_img = cv2.undistort(distorted_img, camera_matrix, dist_coeffs, None, camera_matrix)
|
||||
# cv2.imwrite("distorted_img.png", undistorted_img)
|
||||
# undistorted_img = cv2.imread("distorted_img.png")
|
||||
# cv2.imshow("1", undistorted_img)
|
||||
# cv2.waitKey(0)
|
||||
return undistorted_img
|
||||
|
||||
|
||||
def _origin_project(pcd):
|
||||
# 删除人才
|
||||
pts_3d = []
|
||||
for point_3d in np.asarray(pcd.points):
|
||||
#筛选x,y,z坐标
|
||||
if -2.5 < point_3d[0] < 2.5 and -2.5 < point_3d[1] < 2.5 and point_3d[2] < 3.7:
|
||||
pts_3d.append((point_3d[0], point_3d[1], point_3d[2]))
|
||||
# pts_3d = np.asarray(pcd.points).tolist()
|
||||
|
||||
# 3D投影2D
|
||||
R_mat = pcd.get_rotation_matrix_from_xyz((0.0, 0.0, 0.0))
|
||||
rvec, _ = cv2.Rodrigues(R_mat)
|
||||
tvec = np.float64([0.0, 0.0, 0.0])
|
||||
camera_matrix = np.float64([[fx, 0, cx],
|
||||
[0, fy, cy],
|
||||
[0, 0, 1]])
|
||||
distCoeffs = np.float64([k1, k2, p1, p2, k3])
|
||||
pts_2d, _ = cv2.projectPoints(np.array(pts_3d), rvec, tvec, camera_matrix, distCoeffs)
|
||||
|
||||
# 2D图像绘制
|
||||
max_depth, min_depth = get_pcd_depth_range(pts_3d)
|
||||
image_project = np.zeros((h,w,3), dtype=np.uint8)
|
||||
for i, point_2d in enumerate(pts_2d):
|
||||
x, y = point_2d.ravel()
|
||||
x, y = int(x), int(y)
|
||||
if 0 <= x < image_project.shape[1] and 0 <= y < image_project.shape[0]:
|
||||
cur_depth = pts_3d[i][2]
|
||||
color = get_color(cur_depth, max_depth, min_depth) # 根据深度获取颜色
|
||||
image_project[y, x] = color
|
||||
|
||||
image_project = cv2.resize(image_project, (int(934), int(700)), interpolation=cv2.INTER_LINEAR)
|
||||
# cv2.imshow("project image", image_project)
|
||||
# cv2.waitKey(0)
|
||||
|
||||
image_project = None
|
||||
def _lidar2camera_project(pcd):
|
||||
global image_project
|
||||
# Extract 3D points within a certain range
|
||||
kk = 0.02
|
||||
pts_3d_tmp = []
|
||||
for point_3d in np.asarray(pcd.points):
|
||||
# r = math.sqrt(pow(point_3d[0], 2) + pow(point_3d[1], 2) + pow(point_3d[2], 2))
|
||||
# point_3d[0] = point_3d[0] + point_3d[0] / r * kk
|
||||
# point_3d[1] = point_3d[1] + point_3d[1] / r * kk
|
||||
# point_3d[2] = point_3d[2] + point_3d[2] / r * kk
|
||||
point_3d[2] -= 0.02
|
||||
pts_3d_tmp.append((point_3d[0], point_3d[1], point_3d[2]))
|
||||
|
||||
pts_3d = []
|
||||
# pts_3d = np.asarray(pcd.points).tolist()
|
||||
for point_3d in np.asarray(pts_3d_tmp):
|
||||
if -2.5 < point_3d[0] < 2.5 and -2.5 < point_3d[1] < 2.5 and point_3d[2] < 2.8 : #筛选x,y,z坐标
|
||||
pts_3d.append((point_3d[0], point_3d[1], point_3d[2]))
|
||||
|
||||
pcd1 = o3d.geometry.PointCloud()
|
||||
for pts in pts_3d:
|
||||
pcd1.points.append(pts)
|
||||
o3d.visualization.draw_geometries([pcd1], mesh_show_wireframe=True)
|
||||
|
||||
# R_mat = cloud_load.get_rotation_matrix_from_xyz((0.0, 0.0, 0.0))
|
||||
# rvec, _ = cv2.Rodrigues(R_mat)
|
||||
rvec = np.array([np.array([0.]), np.array([0.]), np.array([0.])])
|
||||
tvec = np.float64([0.0, 0.0, 0.0])
|
||||
camera_matrix = np.float64([[fx, 0, cx],
|
||||
[0, fy, cy],
|
||||
[0, 0, 1]])
|
||||
# distCoeffs = np.float64([k1, k2, p1, p2, k3])
|
||||
distCoeffs = np.float64([0.0, 0.0, 0.0, 0.0, 0.0])
|
||||
pts_2d, _ = cv2.projectPoints(np.array(pts_3d), rvec, tvec, camera_matrix, distCoeffs)
|
||||
|
||||
image_project_zero = np.zeros((h,w,3), dtype=np.uint8)
|
||||
image_project_red = np.zeros((h,w,3), dtype=np.uint8)
|
||||
image_project_red[:] = (0,0,255)
|
||||
# image_project = cv2.imread("./caowei/output.png")
|
||||
image_project = undistort(image_project)
|
||||
image_project_2d_to_3d = np.zeros((h,w,3), dtype=np.float64)
|
||||
# image_project[:] = (255,255,255)
|
||||
|
||||
max_depth, min_depth = get_pcd_depth_range(pts_3d)
|
||||
# sub_depth = max_depth - min_depth
|
||||
for i, point_2d in enumerate(pts_2d):
|
||||
x, y = point_2d.ravel()
|
||||
x, y = int(x), int(y)
|
||||
if 0 <= x < image_project.shape[1] and 0 <= y < image_project.shape[0]:
|
||||
cur_depth = pts_3d[i][2] # 获取当前点的深度
|
||||
color = get_color(cur_depth, max_depth, min_depth) # 根据深度获取颜色
|
||||
# gray = int(cur_depth/sub_depth)
|
||||
color = (255,255,255)
|
||||
image_project_zero[y, x] = color # 设置点云的颜色
|
||||
image_project_2d_to_3d[y, x][0] = pts_3d[i][0]
|
||||
image_project_2d_to_3d[y, x][1] = pts_3d[i][1]
|
||||
image_project_2d_to_3d[y, x][2] = pts_3d[i][2]
|
||||
|
||||
# for test
|
||||
rect_roi = flow_homo.rect_roi(0,0,0,0)
|
||||
flow_homo.api_cal_info(image_project, image_project_2d_to_3d, [rect_roi])
|
||||
|
||||
image_project_zero = cv2.cvtColor(image_project_zero, cv2.COLOR_RGB2GRAY)
|
||||
_, mask = cv2.threshold(image_project_zero, 1,255, cv2.THRESH_BINARY)
|
||||
kernel = np.ones((11,11), np.uint8)
|
||||
mask = cv2.dilate(mask, kernel, iterations=1)
|
||||
# cv2.imshow("project image", mask)
|
||||
image_project = cv2.bitwise_and(image_project_red, image_project, image_project, mask)
|
||||
|
||||
image_project = cv2.resize(image_project, (int(934*1.5), int(700*1.5)), interpolation=cv2.INTER_LINEAR)
|
||||
# cv2.imwrite("./project.jpg", image_project)
|
||||
# cv2.imshow("project image", image_project)
|
||||
# cv2.waitKey(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
cal_project_matrix()
|
||||
|
||||
|
||||
|
||||
image_project = cv2.imread("./caowei/output.png")
|
||||
if image_project is None:
|
||||
print("image_project is None!")
|
||||
exit()
|
||||
# cal_project_matrix()
|
||||
|
||||
# 读取PLY文件
|
||||
ply_file_path = './output.ply'
|
||||
cloud_load = o3d.io.read_point_cloud(ply_file_path)
|
||||
|
||||
|
||||
|
||||
start_time = time.time()
|
||||
pts_3d = np.asarray(cloud_load.points).tolist()
|
||||
# 打印坐标点
|
||||
# xyz_load = np.asarray(cloud_load.points)
|
||||
# print(len(xyz_load))
|
||||
# print(xyz_load)
|
||||
|
||||
# 指定外参：旋转矩阵和平移向量 -0.271002 1.81304 -1.75592
|
||||
Rz = cloud_load.get_rotation_matrix_from_xyz((0, 0, y))
|
||||
Ry = cloud_load.get_rotation_matrix_from_xyz((0, p, 0))
|
||||
Rx = cloud_load.get_rotation_matrix_from_xyz((r, 0, 0))
|
||||
RR = _eulerAnglesToRotationMatrix([1.5707963267948966,-1.5707963267948966,0.0])
|
||||
print(np.pi/2)
|
||||
print(RR)
|
||||
|
||||
# ypr 不等于 xyz 旋转角
|
||||
euler = convert.rot_to_ypr_xyz(R_mat_avg)
|
||||
print(euler)
|
||||
|
||||
t_matrix = np.identity(4)
|
||||
t_matrix[:3, 3] = T_vector
|
||||
# cloud_load.rotate(R_mat_avg)
|
||||
# cloud_load.transform(t_matrix)
|
||||
cloud_load_2 = copy.deepcopy(cloud_load).rotate(Rx, (1,0,0))
|
||||
cloud_load_2.rotate(Ry, (0,1,0))
|
||||
cloud_load_2.rotate(Rz, (0,0,1))
|
||||
cloud_load_2.translate(T_vector)
|
||||
# print(np.asarray(cloud_load.points))
|
||||
# print(np.asarray(cloud_load_2.points))
|
||||
|
||||
# 要用
|
||||
_lidar2camera_project(cloud_load_2)
|
||||
|
||||
# Rx = cloud_load.get_rotation_matrix_from_xyz((1.5708, 0, 0))
|
||||
# Ry = cloud_load.get_rotation_matrix_from_xyz((0, -1.5708, 0))
|
||||
# Rz = cloud_load.get_rotation_matrix_from_xyz((0, 0, 1.5708))
|
||||
# Rxyz = cloud_load.get_rotation_matrix_from_xyz((1.5708, -1.5708, 0.0))
|
||||
# cloud_load_1 = copy.deepcopy(cloud_load).rotate(Rxyz)
|
||||
# cloud_load_2 = copy.deepcopy(cloud_load).rotate(Rx,(1,0,0))
|
||||
# cloud_load_2.rotate(Ry,(0,1,0))
|
||||
# # cloud_load_2 = copy.deepcopy(cloud_load).rotate(Ry,(0,1,0))
|
||||
# # cloud_load_2.rotate(Rz,(0,0,1))
|
||||
# _origin_project(cloud_load_2)
|
||||
end_time = time.time()
|
||||
execution_time = end_time - start_time
|
||||
print(execution_time * 1000)
|
||||
|
||||
# 可以选择将读取的网格可视化
|
||||
vis = o3d.visualization.Visualizer()
|
||||
# 创建三个轴的几何体
|
||||
axes = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0)
|
||||
# 添加轴到可视化窗口
|
||||
vis.add_geometry(axes)
|
||||
|
||||
o3d.visualization.draw_geometries([cloud_load,cloud_load_2], mesh_show_wireframe=True)
|
||||
|
||||
exit()
|
||||
BIN
libMVSDK.so
Normal file
BIN
libMVSDK.so
Normal file
Binary file not shown.
5
lidar_camera_calibration.yaml
Normal file
5
lidar_camera_calibration.yaml
Normal file
@ -0,0 +1,5 @@
|
||||
lidar_camera_calibration:
|
||||
camera_frame_topic: /frontNear/left/image_raw
|
||||
camera_info_topic: /frontNear/left/camera_info
|
||||
velodyne_topic: /velodyne_points
|
||||
|
||||
11
marker_coordinates.txt
Normal file
11
marker_coordinates.txt
Normal file
@ -0,0 +1,11 @@
|
||||
2
|
||||
49.0
|
||||
47.0
|
||||
4.0
|
||||
5.0
|
||||
20.0
|
||||
49.0
|
||||
47.0
|
||||
4.0
|
||||
5.0
|
||||
20.0
|
||||
17
points.txt
Normal file
17
points.txt
Normal file
@ -0,0 +1,17 @@
|
||||
8
|
||||
-0.28833 -0.55161 3.53169
|
||||
0.0474619 -0.264463 3.52989
|
||||
-0.250885 0.0993589 3.53297
|
||||
-0.578516 -0.195354 3.5413
|
||||
0.426036 -0.623606 3.5126
|
||||
0.780823 -0.371227 3.47242
|
||||
0.530741 0.0234222 3.52194
|
||||
0.159776 -0.247036 3.50937
|
||||
1.1191 0.440097 0.441259
|
||||
1.46611 0.726851 0.576376
|
||||
1.15461 1.10508 0.573659
|
||||
0.807603 0.81833 0.438541
|
||||
1.82957 0.348847 0.640063
|
||||
2.20513 0.601418 0.766776
|
||||
1.938 1.01141 0.741323
|
||||
1.56243 0.758839 0.614611
|
||||
BIN
project.jpg
Normal file
BIN
project.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 407 KiB |
16
rct copy.txt
Normal file
16
rct copy.txt
Normal file
@ -0,0 +1,16 @@
|
||||
2528
|
||||
2715
|
||||
3440
|
||||
1574
|
||||
4528
|
||||
2471
|
||||
3607
|
||||
3613
|
||||
4881
|
||||
2549
|
||||
5719
|
||||
1309
|
||||
6876
|
||||
2102
|
||||
6052
|
||||
3367
|
||||
16
rct.txt
Normal file
16
rct.txt
Normal file
@ -0,0 +1,16 @@
|
||||
2598
|
||||
2715
|
||||
3490
|
||||
1574
|
||||
4578
|
||||
2471
|
||||
3677
|
||||
3673
|
||||
4931
|
||||
2549
|
||||
5769
|
||||
1309
|
||||
6886
|
||||
2132
|
||||
6102
|
||||
3327
|
||||
138
rect.py
Normal file
138
rect.py
Normal file
@ -0,0 +1,138 @@
|
||||
# -- coding: utf-8 --
|
||||
import os
|
||||
import sys
|
||||
import threading
|
||||
import ctypes
|
||||
from ctypes import *
|
||||
import platform
|
||||
import time
|
||||
import cv2 # 4.2.0.32
|
||||
import numpy as np
|
||||
|
||||
|
||||
WIDTH = 9344
|
||||
HEIGH = 7000
|
||||
step = 40
|
||||
|
||||
rect_conner_points = []
|
||||
edge_rect_points = []
|
||||
|
||||
|
||||
# 生成一个随机颜色
|
||||
def generate_random_color():
|
||||
color_bgr = np.random.randint(0, 255, size=(3, ))
|
||||
return color_bgr.tolist()
|
||||
|
||||
|
||||
def cal_edge_rect(p1, p2, idx, im):
|
||||
print(p1, p2)
|
||||
tmp = []
|
||||
if idx == 1:
|
||||
tmp.append([p1[0]-step,p1[1]-step])
|
||||
tmp.append([p1[0]-step,p1[1]+step*3])
|
||||
tmp.append([p2[0]+step*3,p2[1]-step])
|
||||
tmp.append([p2[0]-step,p2[1]-step])
|
||||
elif idx == 2:
|
||||
tmp.append([p1[0]+step,p1[1]-step])
|
||||
tmp.append([p1[0]-step*3,p1[1]-step])
|
||||
tmp.append([p2[0]+step,p2[1]+step*3])
|
||||
tmp.append([p2[0]+step,p2[1]-step])
|
||||
elif idx == 3:
|
||||
tmp.append([p1[0]+step,p1[1]+step])
|
||||
tmp.append([p1[0]+step,p1[1]-step*3])
|
||||
tmp.append([p2[0]-step*3,p2[1]+step])
|
||||
tmp.append([p2[0]+step,p2[1]+step])
|
||||
else:
|
||||
tmp.append([p1[0]+step*3,p1[1]+step])
|
||||
tmp.append([p1[0]-step,p1[1]+step])
|
||||
tmp.append([p2[0]-step,p2[1]+step])
|
||||
tmp.append([p2[0]-step,p2[1]-step*3])
|
||||
|
||||
edge_rect_points.append(tmp)
|
||||
pts = np.array(tmp, np.int32)
|
||||
# pts = [pts]
|
||||
color = (0, 255, 0)
|
||||
color = generate_random_color()
|
||||
# if idx == 4:
|
||||
cv2.polylines(im, [pts], True, color, 5)
|
||||
|
||||
|
||||
def cal_edge_rect2(p1, p2, idx, im):
|
||||
print(p1, p2)
|
||||
tmp = []
|
||||
if idx == 1:
|
||||
tmp.append([p1[0]-step,p1[1]])
|
||||
tmp.append([p1[0]+step,p1[1]])
|
||||
tmp.append([p2[0],p2[1]+step])
|
||||
tmp.append([p2[0],p2[1]-step])
|
||||
elif idx == 2:
|
||||
tmp.append([p1[0],p1[1]-step])
|
||||
tmp.append([p1[0],p1[1]+step])
|
||||
tmp.append([p2[0]-step,p2[1]])
|
||||
tmp.append([p2[0]+step,p2[1]])
|
||||
elif idx == 3:
|
||||
tmp.append([p1[0]+step,p1[1]])
|
||||
tmp.append([p1[0]-step,p1[1]])
|
||||
tmp.append([p2[0],p2[1]-step])
|
||||
tmp.append([p2[0],p2[1]+step])
|
||||
else:
|
||||
tmp.append([p1[0],p1[1]+step])
|
||||
tmp.append([p1[0],p1[1]-step])
|
||||
tmp.append([p2[0]+step,p2[1]])
|
||||
tmp.append([p2[0]-step,p2[1]])
|
||||
|
||||
edge_rect_points.append(tmp)
|
||||
pts = np.array(tmp, np.int32)
|
||||
# pts = [pts]
|
||||
color = (0, 255, 0)
|
||||
color = generate_random_color()
|
||||
# if idx == 4:
|
||||
cv2.polylines(im, [pts], True, color, 5)
|
||||
|
||||
|
||||
with open('rct.txt', 'r') as file:
|
||||
|
||||
# im = np.zeros((HEIGH,WIDTH,3), np.uint8)
|
||||
im = cv2.imread("cloud.png")
|
||||
# im = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
|
||||
# _,im= cv2.threshold(im, 20, 255, cv2. THRESH_BINARY)
|
||||
# im = cv2.merge([im, im, im])
|
||||
|
||||
while True:
|
||||
line1 = file.readline()
|
||||
if not line1: break
|
||||
line2 = file.readline()
|
||||
|
||||
x = int(line1) + 80 +40
|
||||
y = int(line2) + 40
|
||||
|
||||
rect_conner_points.append([x, y])
|
||||
cv2.circle(im, (x,y), 25, (255, 0, 0) , -1)
|
||||
# print(rect_conner_points)
|
||||
rpts = rect_conner_points
|
||||
|
||||
cal_edge_rect2(rpts[0], rpts[1], 1, im)
|
||||
cal_edge_rect2(rpts[1], rpts[2], 2, im)
|
||||
cal_edge_rect2(rpts[2], rpts[3], 3, im)
|
||||
cal_edge_rect2(rpts[3], rpts[0], 4, im)
|
||||
cal_edge_rect2(rpts[4], rpts[5], 1, im)
|
||||
cal_edge_rect2(rpts[5], rpts[6], 2, im)
|
||||
cal_edge_rect2(rpts[6], rpts[7], 3, im)
|
||||
cal_edge_rect2(rpts[7], rpts[4], 4, im)
|
||||
print(edge_rect_points)
|
||||
|
||||
im = cv2.resize(im, (1800, 1400))
|
||||
cv2.imshow("im", im)
|
||||
cv2.waitKey(0)
|
||||
cv2.destroiyAllWindows()
|
||||
|
||||
with open('self_define_point.txt', 'w') as file2:
|
||||
print("11")
|
||||
for i in range(8):
|
||||
for y in range(4):
|
||||
print(str(edge_rect_points[i][y][0]))
|
||||
print(str(edge_rect_points[i][y][1]))
|
||||
file2.write(str(edge_rect_points[i][y][0])+'\n')
|
||||
file2.write(str(edge_rect_points[i][y][1])+'\n')
|
||||
|
||||
exit()
|
||||
64
rst.txt
Normal file
64
rst.txt
Normal file
@ -0,0 +1,64 @@
|
||||
2608
|
||||
2715
|
||||
2708
|
||||
2715
|
||||
3570
|
||||
1624
|
||||
3570
|
||||
1524
|
||||
3570
|
||||
1524
|
||||
3570
|
||||
1624
|
||||
4608
|
||||
2471
|
||||
4708
|
||||
2471
|
||||
4708
|
||||
2471
|
||||
4608
|
||||
2471
|
||||
3737
|
||||
3563
|
||||
3737
|
||||
3663
|
||||
3737
|
||||
3663
|
||||
3737
|
||||
3563
|
||||
2708
|
||||
2715
|
||||
2608
|
||||
2715
|
||||
4961
|
||||
2549
|
||||
5061
|
||||
2549
|
||||
5849
|
||||
1359
|
||||
5849
|
||||
1259
|
||||
5849
|
||||
1259
|
||||
5849
|
||||
1359
|
||||
6956
|
||||
2152
|
||||
7056
|
||||
2152
|
||||
7056
|
||||
2152
|
||||
6956
|
||||
2152
|
||||
6182
|
||||
3317
|
||||
6182
|
||||
3417
|
||||
6182
|
||||
3417
|
||||
6182
|
||||
3317
|
||||
5061
|
||||
2549
|
||||
4961
|
||||
2549
|
||||
64
self_define_point.txt
Normal file
64
self_define_point.txt
Normal file
@ -0,0 +1,64 @@
|
||||
2673
|
||||
2755
|
||||
2723
|
||||
2755
|
||||
3610
|
||||
1639
|
||||
3610
|
||||
1589
|
||||
3610
|
||||
1589
|
||||
3610
|
||||
1639
|
||||
4673
|
||||
2511
|
||||
4723
|
||||
2511
|
||||
4723
|
||||
2511
|
||||
4673
|
||||
2511
|
||||
3777
|
||||
3628
|
||||
3777
|
||||
3678
|
||||
3777
|
||||
3678
|
||||
3777
|
||||
3628
|
||||
2723
|
||||
2755
|
||||
2673
|
||||
2755
|
||||
5026
|
||||
2589
|
||||
5076
|
||||
2589
|
||||
5889
|
||||
1374
|
||||
5889
|
||||
1324
|
||||
5889
|
||||
1324
|
||||
5889
|
||||
1374
|
||||
7021
|
||||
2192
|
||||
7071
|
||||
2192
|
||||
7071
|
||||
2192
|
||||
7021
|
||||
2192
|
||||
6222
|
||||
3382
|
||||
6222
|
||||
3432
|
||||
6222
|
||||
3432
|
||||
6222
|
||||
3382
|
||||
5076
|
||||
2589
|
||||
5026
|
||||
2589
|
||||
2
transform.txt
Normal file
2
transform.txt
Normal file
@ -0,0 +1,2 @@
|
||||
26=(4184.22,2816.69) (3784.89,3306.51) (3299.05,2902.9) (3694.91,2412.14) Txyz=1.00735 0.801781 0.48244 Rxyz=-0.363316 1.80462 -1.63302
|
||||
582=(6575.88,2558.57) (6207.98,3087.54) (5682.26,2725.23) (6048.3,2195.78) Txyz=1.75862 0.722306 0.662323 Rxyz=-0.276921 1.80714 -1.75415
|
||||
47
vidgear_client.py
Normal file
47
vidgear_client.py
Normal file
@ -0,0 +1,47 @@
|
||||
from vidgear.gears import NetGear
|
||||
import cv2
|
||||
import time
|
||||
|
||||
# 设置，这里都是消息接收的方式，直接默认就行了，不用管。
|
||||
options = {
|
||||
"flag": 0, "copy": False, "track": False,
|
||||
# "jpeg_compression": "GRAY",
|
||||
"jpeg_compression": True,
|
||||
"jpeg_compression_quality": 90,
|
||||
"jpeg_compression_fastdct": True,
|
||||
"jpeg_compression_fastupsample": True
|
||||
}
|
||||
|
||||
|
||||
# address设置为客户端的ip，port设置为客户端的端口
|
||||
client = NetGear(
|
||||
address="192.168.101.46",
|
||||
port="5454",
|
||||
protocol="tcp",
|
||||
pattern=1,
|
||||
receive_mode=True,
|
||||
logging=True,
|
||||
**options
|
||||
)
|
||||
|
||||
idx = 0
|
||||
while True:
|
||||
# 收取数据
|
||||
frame = client.recv()
|
||||
|
||||
if frame is None:
|
||||
break
|
||||
|
||||
cv2.imshow("Output Frame", frame)
|
||||
idx += 1
|
||||
if idx == 30:
|
||||
print(time.time())
|
||||
idx = 0
|
||||
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key == ord("q"):
|
||||
break
|
||||
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
client.close()
|
||||
78
wurenji.py
Normal file
78
wurenji.py
Normal file
@ -0,0 +1,78 @@
|
||||
# -- coding: utf-8 --
|
||||
from ctypes import *
|
||||
import cv2 # 4.2.0.32
|
||||
import numpy as np
|
||||
from datetime import datetime
|
||||
import time
|
||||
|
||||
drawing = False # 鼠标是否按下
|
||||
ix, iy = -1, -1 # 初始坐标
|
||||
roi = None # ROI区域
|
||||
|
||||
|
||||
def _cal_roi(img):
|
||||
# 获取当前时间
|
||||
now = datetime.now()
|
||||
# 转换为时间戳（秒）
|
||||
timestamp = time.mktime(now.timetuple())
|
||||
# 转换为时间戳（毫秒）
|
||||
timestamp_ms = int(timestamp * 1000)
|
||||
# 获取当前时间的毫秒部分
|
||||
milliseconds = now.microsecond // 1000
|
||||
# 合并秒和毫秒
|
||||
timestamp_ms += milliseconds
|
||||
date_str = time.strftime("%Y_%m_%d_%H_%M_%S", time.localtime(timestamp))
|
||||
path = "./wurenji/" + date_str + ".jpg"
|
||||
rst = cv2.imwrite(path, img)
|
||||
print(date_str)
|
||||
print("cv2.imwrite rst: " + str(rst))
|
||||
|
||||
|
||||
# 鼠标回调函数
|
||||
def cal_roi(event, x, y, flags, param):
|
||||
global ix, iy, drawing, roi
|
||||
|
||||
if event == cv2.EVENT_LBUTTONDOWN:
|
||||
drawing = True
|
||||
ix, iy = x, y
|
||||
|
||||
# elif event == cv2.EVENT_MOUSEMOVE:
|
||||
# if drawing:
|
||||
# img = param
|
||||
# cv2.rectangle(img, (ix, iy), (x, y), (0, 255, 0), 1)
|
||||
# roi = (min(ix, x), min(iy, y), abs(ix - x), abs(iy - y))
|
||||
|
||||
elif event == cv2.EVENT_LBUTTONUP:
|
||||
drawing = False
|
||||
roi = (min(ix, x), min(iy, y), abs(ix - x), abs(iy - y))
|
||||
if roi:
|
||||
x = roi[0]
|
||||
y = roi[1]
|
||||
w = roi[2]
|
||||
h = roi[3]
|
||||
img = param
|
||||
_roi_im = img[y:y+h, x:x+w].copy()
|
||||
# cv2.rectangle(img, (roi[0], roi[1]), (roi[0] + roi[2], roi[1] + roi[3]), (0, 255, 0), 1)
|
||||
_cal_roi(_roi_im)
|
||||
|
||||
|
||||
# 主程序
|
||||
img = cv2.imread('project.jpg') # 替换为你的图片路径
|
||||
cv2.imshow('image', img)
|
||||
cv2.setMouseCallback('image', cal_roi, param=img)
|
||||
|
||||
while True:
|
||||
k = cv2.waitKey(1) & 0xFF
|
||||
if k == 27: # ESC键退出
|
||||
break
|
||||
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
# roi变量现在包含了你勾选的ROI区域
|
||||
if roi:
|
||||
x, y, w, h = roi
|
||||
# 处理ROI区域
|
||||
# roi_img = img[y:y+h, x:x+w]
|
||||
# cv2.imshow('ROI', roi_img)
|
||||
# cv2.waitKey(0)
|
||||
# cv2.destroyAllWindows()
|
||||
Loading…
Reference in New Issue
Block a user