BOOL CSITTable::Decode( BYTE* data, DWORD dataSize, DWORD* decodeReadSize )
{
if( data == NULL ){
return FALSE;
}
Clear();
//////////////////////////////////////////////////////
//サイズのチェック
//最低限table_idとsection_length+CRCのサイズは必須
if( dataSize < 7 ){
return FALSE;
}
//->サイズのチェック
DWORD readSize = 0;
//////////////////////////////////////////////////////
//解析処理
table_id = data[0];
section_syntax_indicator = (data[1]&0x80)>>7;
section_length = ((WORD)data[1]&0x0F)<<8 | data[2];
readSize+=3;
if( section_syntax_indicator != 1 ){
//固定値がおかしい
_OutputDebugString( L"++CSITTable:: section_syntax err" );
return FALSE;
}
if( table_id != 0x7F ){
//table_idがおかしい
_OutputDebugString( L"++CSITTable:: table_id err 0x7F != 0x%02X", table_id );
return FALSE;
}
if( readSize+section_length > dataSize && section_length > 3){
//サイズ異常
_OutputDebugString( L"++CSITTable:: size err %d > %d", readSize+section_length, dataSize );
return FALSE;
}
//CRCチェック
crc32 = ((DWORD)data[3+section_length-4])<<24 |
((DWORD)data[3+section_length-3])<<16 |
((DWORD)data[3+section_length-2])<<8 |
data[3+section_length-1];
if( crc32 != _Crc32(3+section_length-4, data) ){
_OutputDebugString( L"++CSITTable:: CRC err" );
return FALSE;
}
if( section_length > 8 ){
version_number = (data[readSize+2]&0x3E)>>1;
current_next_indicator = data[readSize+2]&0x01;
section_number = data[readSize+3];
last_section_number = data[readSize+4];
transmission_info_loop_length = ((WORD)data[readSize+5]&0x0F)<<8 | data[readSize+6];
readSize += 7;
if( readSize+transmission_info_loop_length <= (DWORD)section_length+3-4 && transmission_info_loop_length > 0){
CDescriptor descriptor;
if( descriptor.Decode( data+readSize, transmission_info_loop_length, &descriptorList, NULL ) == FALSE ){
_OutputDebugString( L"++CSITTable:: descriptor err" );
return FALSE;
}
readSize+=transmission_info_loop_length;
}
while( readSize < (DWORD)section_length+3-4 ){
SERVICE_LOOP_DATA* item = new SERVICE_LOOP_DATA;
item->service_id = ((WORD)data[readSize])<<8 | data[readSize+1];
item->running_status = (data[readSize+2]&0x70)>>4;
item->service_loop_length = ((WORD)data[readSize+2]&0x0F)<<8 | data[readSize+3];
readSize += 4;
if( readSize+item->service_loop_length <= (DWORD)section_length+3-4 && item->service_loop_length > 0){
CDescriptor descriptor;
if( descriptor.Decode( data+readSize, item->service_loop_length, &(item->descriptorList), NULL ) == FALSE ){
_OutputDebugString( L"++CSITTable:: descriptor2 err" );
return FALSE;
}
}
readSize+=item->service_loop_length;
serviceLoopList.push_back(item);
}
}else{
BOOL CTOTTable::Decode( BYTE* data, DWORD dataSize, DWORD* decodeReadSize )
{
if( data == NULL ){
return FALSE;
}
Clear();
//////////////////////////////////////////////////////
//サイズのチェック
//最低限table_idとsection_length+CRCのサイズは必須
if( dataSize < 7 ){
return FALSE;
}
//->サイズのチェック
DWORD readSize = 0;
//////////////////////////////////////////////////////
//解析処理
table_id = data[0];
section_syntax_indicator = (data[1]&0x80)>>7;
section_length = ((WORD)data[1]&0x0F)<<8 | data[2];
readSize+=3;
if( section_syntax_indicator != 0 ){
//固定値がおかしい
_OutputDebugString( L"++CTOTTable:: section_syntax err" );
return FALSE;
}
if( table_id != 0x73 ){
//table_idがおかしい
_OutputDebugString( L"++CTOTTable:: table_id err 0x%02X", table_id );
return FALSE;
}
if( readSize+section_length > dataSize && section_length > 3){
//サイズ異常
_OutputDebugString( L"++CTOTTable:: size err %d > %d", readSize+section_length, dataSize );
return FALSE;
}
//CRCチェック
crc32 = ((DWORD)data[3+section_length-4])<<24 |
((DWORD)data[3+section_length-3])<<16 |
((DWORD)data[3+section_length-2])<<8 |
data[3+section_length-1];
if( crc32 != _Crc32(3+section_length-4, data) ){
_OutputDebugString( L"++CTOTTable:: CRC err" );
return FALSE;
}
if( section_length > 4 ){
DWORD mjd = ((DWORD)data[readSize])<<8 | data[readSize+1];
_MJDtoSYSTEMTIME(mjd, &jst_time);
jst_time.wHour = (WORD)_BCDtoDWORD(data+readSize+2, 1, 2);
jst_time.wMinute = (WORD)_BCDtoDWORD(data+readSize+3, 1, 2);
jst_time.wSecond = (WORD)_BCDtoDWORD(data+readSize+4, 1, 2);
readSize += 5;
descriptors_loop_length = ((WORD)data[readSize]&0x0F)<<8 | data[readSize+1];
readSize += 2;
if( readSize+descriptors_loop_length <= (DWORD)section_length+3-4 && descriptors_loop_length > 0){
CDescriptor descriptor;
if( descriptor.Decode( data+readSize, descriptors_loop_length, &descriptorList, NULL ) == FALSE ){
_OutputDebugString( L"++CTOTTable:: descriptor2 err" );
return FALSE;
}
}
readSize+=descriptors_loop_length;
}else{
return FALSE;
}
//->解析処理
if( decodeReadSize != NULL ){
*decodeReadSize = 3+section_length;
}
return TRUE;
}
const std::shared_ptr< std::vector< CLandmark* > > CTrackerStereoMotionModel::_getNewLandmarks( const UIDFrame& p_uFrame,
cv::Mat& p_matDisplay,
const cv::Mat& p_matImageLEFT,
const cv::Mat& p_matImageRIGHT,
const Eigen::Isometry3d& p_matTransformationWORLDtoLEFT,
const Eigen::Isometry3d& p_matTransformationLEFTtoWORLD,
const Eigen::Vector3d& p_vecRotation )
{
//ds precompute extrinsics
const MatrixProjection matProjectionWORLDtoLEFT( m_pCameraLEFT->m_matProjection*p_matTransformationWORLDtoLEFT.matrix( ) );
//ds solution holder
std::shared_ptr< std::vector< CLandmark* > > vecNewLandmarks( std::make_shared< std::vector< CLandmark* > >( ) );
//ds detect new keypoints
//const std::shared_ptr< std::vector< cv::KeyPoint > > vecKeyPoints( m_cDetector.detectKeyPointsTilewise( p_matImageLEFT, matMask ) );
std::vector< cv::KeyPoint > vecKeyPoints;
m_pDetector->detect( p_matImageLEFT, vecKeyPoints, m_cMatcher.getMaskActiveLandmarks( p_matTransformationWORLDtoLEFT, p_matDisplay ) );
//ds compute descriptors for the keypoints
CDescriptor matReferenceDescriptors;
//m_pExtractor->compute( p_matImageLEFT, *vecKeyPoints, matReferenceDescriptors );
m_pExtractor->compute( p_matImageLEFT, vecKeyPoints, matReferenceDescriptors );
//ds process the keypoints and see if we can use them as landmarks
for( uint32_t u = 0; u < vecKeyPoints.size( ); ++u )
{
//ds current points
const cv::KeyPoint cKeyPointLEFT( vecKeyPoints[u] );
const cv::Point2f ptLandmarkLEFT( cKeyPointLEFT.pt );
const CDescriptor matDescriptorLEFT( matReferenceDescriptors.row(u) );
try
{
//ds triangulate the point
const CMatchTriangulation cMatch( m_pTriangulator->getPointTriangulatedCompactInRIGHT( p_matImageRIGHT, cKeyPointLEFT, matDescriptorLEFT ) );
const CPoint3DCAMERA vecPointTriangulatedLEFT( cMatch.vecPointXYZCAMERA );
const CDescriptor matDescriptorRIGHT( cMatch.matDescriptorCAMERA );
//ds check depth
const double dDepthMeters( vecPointTriangulatedLEFT.z( ) );
//ds check if point is in front of camera an not more than a defined distance away
if( m_dMinimumDepthMeters < dDepthMeters && m_dMaximumDepthMeters > dDepthMeters )
{
//ds compute triangulated point in world frame
const CPoint3DWORLD vecPointTriangulatedWORLD( p_matTransformationLEFTtoWORLD*vecPointTriangulatedLEFT );
//ds landmark right
const cv::Point2f ptLandmarkRIGHT( cMatch.ptUVCAMERA );
//ds allocate a new landmark and add the current position
CLandmark* pLandmark( new CLandmark( m_uAvailableLandmarkID,
matDescriptorLEFT,
cMatch.matDescriptorCAMERA,
cKeyPointLEFT.size,
vecPointTriangulatedWORLD,
m_pCameraLEFT->getNormalHomogenized( ptLandmarkLEFT ),
ptLandmarkLEFT,
ptLandmarkRIGHT,
vecPointTriangulatedLEFT,
p_matTransformationLEFTtoWORLD.translation( ),
p_vecRotation,
matProjectionWORLDtoLEFT,
p_uFrame ) );
//ds log creation
CLogger::CLogLandmarkCreation::addEntry( p_uFrame, pLandmark, dDepthMeters, ptLandmarkLEFT, ptLandmarkRIGHT );
//ds add to newly detected
vecNewLandmarks->push_back( pLandmark );
//ds next landmark id
++m_uAvailableLandmarkID;
//ds draw detected point
cv::line( p_matDisplay, ptLandmarkLEFT, cv::Point2f( ptLandmarkLEFT.x+m_pCameraSTEREO->m_uPixelWidth, ptLandmarkLEFT.y ), CColorCodeBGR( 175, 175, 175 ) );
cv::circle( p_matDisplay, ptLandmarkLEFT, 2, CColorCodeBGR( 0, 255, 0 ), -1 );
//cv::circle( p_matDisplay, ptLandmarkLEFT, cLandmark->dKeyPointSize, CColorCodeBGR( 255, 0, 0 ), 1 );
cv::putText( p_matDisplay, std::to_string( pLandmark->uID ) , cv::Point2d( ptLandmarkLEFT.x+pLandmark->dKeyPointSize, ptLandmarkLEFT.y+pLandmark->dKeyPointSize ), cv::FONT_HERSHEY_PLAIN, 0.5, CColorCodeBGR( 0, 0, 255 ) );
//ds draw reprojection of triangulation
cv::circle( p_matDisplay, cv::Point2d( ptLandmarkRIGHT.x+m_pCameraSTEREO->m_uPixelWidth, ptLandmarkRIGHT.y ), 2, CColorCodeBGR( 255, 0, 0 ), -1 );
}
else
{
cv::circle( p_matDisplay, ptLandmarkLEFT, 2, CColorCodeBGR( 0, 0, 255 ), -1 );
//cv::circle( p_matDisplay, ptLandmarkLEFT, cKeyPoint.size, CColorCodeBGR( 0, 0, 255 ) );
//std::printf( "<CTrackerStereoMotionModel>(_getNewLandmarks) could not find match for keypoint (invalid depth: %f m)\n", vecPointTriangulatedLEFT(2) );
}
}
catch( const CExceptionNoMatchFound& p_cException )
{
cv::circle( p_matDisplay, ptLandmarkLEFT, 2, CColorCodeBGR( 0, 0, 255 ), -1 );
//cv::circle( p_matDisplay, ptLandmarkLEFT, cKeyPoint.size, CColorCodeBGR( 0, 0, 255 ) );
//std::printf( "<CTrackerStereoMotionModel>(_getNewLandmarks) could not find match for keypoint (%s)\n", p_cException.what( ) );
}
}
//std::printf( "<CTrackerStereoMotionModel>(_getNewLandmarks) added new landmarks: %lu/%lu\n", vecNewLandmarks->size( ), vecKeyPoints.size( ) );
//ds return found landmarks
return vecNewLandmarks;
}
BOOL CEITTable_SD::Decode( BYTE* data, DWORD dataSize, DWORD* decodeReadSize )
{
if( data == NULL ){
return FALSE;
}
Clear();
//////////////////////////////////////////////////////
//サイズのチェック
//最低限table_idとsection_length+CRCのサイズは必須
if( dataSize < 7 ){
return FALSE;
}
//->サイズのチェック
DWORD readSize = 0;
//////////////////////////////////////////////////////
//解析処理
table_id = data[0];
section_syntax_indicator = (data[1]&0x80)>>7;
section_length = ((WORD)data[1]&0x0F)<<8 | data[2];
readSize+=3;
if( section_syntax_indicator != 1 ){
//固定値がおかしい
_OutputDebugString( L"++CEITTable_SD:: section_syntax err" );
return FALSE;
}
if( table_id != 0xA4 && table_id != 0xA7 ){
//table_idがおかしい
_OutputDebugString( L"++CEITTable_SD:: table_id err 0x%02X", table_id );
return FALSE;
}
if( readSize+section_length > dataSize && section_length > 3){
//サイズ異常
_OutputDebugString( L"++CEITTable_SD:: size err %d > %d", readSize+section_length, dataSize );
return FALSE;
}
//CRCチェック
crc32 = ((DWORD)data[3+section_length-4])<<24 |
((DWORD)data[3+section_length-3])<<16 |
((DWORD)data[3+section_length-2])<<8 |
data[3+section_length-1];
if( crc32 != _Crc32(3+section_length-4, data) ){
_OutputDebugString( L"++CEITTable_SD:: CRC err" );
return FALSE;
}
if( section_length > 4 ){
service_id = ((WORD)data[readSize])<<8 | data[readSize+1];
version_number = (data[readSize+2]&0x3E)>>1;
current_next_indicator = data[readSize+2]&0x01;
section_number = data[readSize+3];
last_section_number = data[readSize+4];
transport_stream_id = ((WORD)data[readSize+5])<<8 | data[readSize+6];
original_network_id = ((WORD)data[readSize+7])<<8 | data[readSize+8];
readSize += 9;
while( readSize < (DWORD)section_length+3-4 ){
EVENT_INFO_DATA* item = new EVENT_INFO_DATA;
item->event_id = ((WORD)data[readSize])<<8 | data[readSize+1];
if( data[readSize+2] == 0xFF && data[readSize+3] == 0xFF && data[readSize+4] == 0xFF &&
data[readSize+5] == 0xFF && data[readSize+6] == 0xFF )
{
item->StartTimeFlag = FALSE;
}else{
item->StartTimeFlag = TRUE;
DWORD mjd = ((DWORD)data[readSize+2])<<8 | data[readSize+3];
_MJDtoSYSTEMTIME(mjd, &(item->start_time));
item->start_time.wHour = (WORD)_BCDtoDWORD(data+readSize+4, 1, 2);
item->start_time.wMinute = (WORD)_BCDtoDWORD(data+readSize+5, 1, 2);
item->start_time.wSecond = (WORD)_BCDtoDWORD(data+readSize+6, 1, 2);
}
readSize+=7;
if( data[readSize] == 0xFF && data[readSize+1] == 0xFF && data[readSize+2] == 0xFF)
{
item->DurationFlag = FALSE;
}else{
item->DurationFlag = TRUE;
item->durationHH = (WORD)_BCDtoDWORD(data+readSize, 1, 2);
item->durationMM = (WORD)_BCDtoDWORD(data+readSize+1, 1, 2);
item->durationSS = (WORD)_BCDtoDWORD(data+readSize+2, 1, 2);
}
readSize+=3;
item->running_status = (data[readSize]&0xE0)>>5;
item->free_CA_mode = (data[readSize]&0x10)>>4;
item->descriptors_loop_length = ((WORD)data[readSize]&0x0F)<<8 | data[readSize+1];
readSize += 2;
if( readSize+item->descriptors_loop_length <= (DWORD)section_length+3-4 && item->descriptors_loop_length > 0){
if( original_network_id == 0x0001 || original_network_id == 0x0003 ){
SDDecode( data+readSize, item->descriptors_loop_length, &(item->descriptorList), NULL );
}else{
CDescriptor descriptor;
if( descriptor.Decode( data+readSize, item->descriptors_loop_length, &(item->descriptorList), NULL ) == FALSE ){
_OutputDebugString( L"++CEITTable_SD:: descriptor2 err" );
return FALSE;
}
}
}
readSize+=item->descriptors_loop_length;
eventInfoList.push_back(item);
}
}else{
BOOL CNITTable::Decode( BYTE* data, DWORD dataSize, DWORD* decodeReadSize )
{
if( data == NULL ){
return FALSE;
}
Clear();
//////////////////////////////////////////////////////
//サイズのチェック
//最低限table_idとsection_length+CRCのサイズは必須
if( dataSize < 7 ){
return FALSE;
}
//->サイズのチェック
DWORD readSize = 0;
//////////////////////////////////////////////////////
//解析処理
table_id = data[0];
section_syntax_indicator = (data[1]&0x80)>>7;
section_length = ((WORD)data[1]&0x0F)<<8 | data[2];
readSize+=3;
if( section_syntax_indicator != 1 ){
//固定値がおかしい
_OutputDebugString( L"++CNITTable:: section_syntax err" );
return FALSE;
}
if( table_id != 0x40 && table_id != 0x41 ){
//table_idがおかしい
_OutputDebugString( L"++CNITTable:: table_id err 0x%02X", table_id );
return FALSE;
}
if( readSize+section_length > dataSize && section_length > 3){
//サイズ異常
_OutputDebugString( L"++CNITTable:: size err %d > %d", readSize+section_length, dataSize );
return FALSE;
}
//CRCチェック
crc32 = ((DWORD)data[3+section_length-4])<<24 |
((DWORD)data[3+section_length-3])<<16 |
((DWORD)data[3+section_length-2])<<8 |
data[3+section_length-1];
if( crc32 != _Crc32(3+section_length-4, data) ){
_OutputDebugString( L"++CNITTable:: CRC err" );
return FALSE;
}
if( section_length > 8 ){
network_id = ((WORD)data[readSize])<<8 | data[readSize+1];
version_number = (data[readSize+2]&0x3E)>>1;
current_next_indicator = data[readSize+2]&0x01;
section_number = data[readSize+3];
last_section_number = data[readSize+4];
network_descriptors_length = ((WORD)data[readSize+5]&0x0F)<<8 | data[readSize+6];
readSize += 7;
if( readSize+network_descriptors_length <= (DWORD)section_length+3-4 && network_descriptors_length > 0){
if( network_id == 0x0001 || network_id == 0x0003 ){
SDDecode( data+readSize, network_descriptors_length, &descriptorList, NULL );
}else{
CDescriptor descriptor;
if( descriptor.Decode( data+readSize, network_descriptors_length, &descriptorList, NULL ) == FALSE ){
_OutputDebugString( L"++CNITTable:: descriptor err" );
return FALSE;
}
}
readSize+=network_descriptors_length;
}
transport_stream_loop_length = ((WORD)data[readSize]&0x0F)<<8 | data[readSize+1];
readSize += 2;
WORD tsLoopReadSize = 0;
while( readSize < (DWORD)section_length+3-4 && tsLoopReadSize < transport_stream_loop_length){
TS_INFO_DATA* item = new TS_INFO_DATA;
item->transport_stream_id = ((WORD)data[readSize])<<8 | data[readSize+1];
item->original_network_id = ((WORD)data[readSize+2])<<8 | data[readSize+3];
item->transport_descriptors_length = ((WORD)data[readSize+4]&0x0F)<<8 | data[readSize+5];
readSize += 6;
if( readSize+item->transport_descriptors_length <= (DWORD)section_length+3-4 && item->transport_descriptors_length > 0){
CDescriptor descriptor;
if( descriptor.Decode( data+readSize, item->transport_descriptors_length, &(item->descriptorList), NULL ) == FALSE ){
_OutputDebugString( L"++CNITTable:: descriptor2 err" );
return FALSE;
}
}
readSize+=item->transport_descriptors_length;
tsLoopReadSize += 6 + item->transport_descriptors_length;
TSInfoList.push_back(item);
}
}else{
void computeDepth( const cv::Mat& p_matImageLEFT, const cv::Mat& p_matImageRIGHT )
{
//ds preprocessed images
cv::Mat matPreprocessedLEFT;
cv::Mat matPreprocessedRIGHT;
//ds preprocess images
cv::equalizeHist( p_matImageLEFT, matPreprocessedLEFT );
cv::equalizeHist( p_matImageRIGHT, matPreprocessedRIGHT );
g_pCameraSTEREO->undistortAndrectify( matPreprocessedLEFT, matPreprocessedRIGHT );
//ds get images into triple channel mats (display only)
cv::Mat matDisplayLEFT;
cv::Mat matDisplayRIGHT;
//ds get images to triple channel for colored display
cv::cvtColor( matPreprocessedLEFT, matDisplayLEFT, cv::COLOR_GRAY2BGR );
cv::cvtColor( matPreprocessedRIGHT, matDisplayRIGHT, cv::COLOR_GRAY2BGR );
//ds detect features
std::vector< cv::KeyPoint > vecKeyPoints;
g_pDetector->detect( matPreprocessedLEFT, vecKeyPoints );
//ds compute descriptors for the keypoints
CDescriptor matReferenceDescriptors;
g_pExtractor->compute( matPreprocessedLEFT, vecKeyPoints, matReferenceDescriptors );
//ds count active points
uint64_t uActivePoints( 0 );
//ds errors
double dAverageErrorSVDLS( 0.0 );
double dAverageErrorQRLS( 0.0 );
double dAverageErrorEDS( 0.0 );
double dAverageErrorSVDDLT( 0.0 );
uint64_t uPointsSVDDLS( 0 );
uint64_t uPointsQRLS( 0 );
uint64_t uPointsEDS( 0 );
uint64_t uPointsSVDDLT( 0 );
//ds process the keypoints and see if we can use them as landmarks
for( uint32_t u = 0; u < vecKeyPoints.size( ); ++u )
{
//ds current points
const cv::KeyPoint cKeyPoint( vecKeyPoints[u] );
const cv::Point2f ptLandmarkLEFT( cKeyPoint.pt );
const CDescriptor& matReferenceDescriptorLEFT( matReferenceDescriptors.row(u) );
//ds draw detected point
cv::circle( matDisplayLEFT, ptLandmarkLEFT, 2, CColorCodeBGR( 255, 0, 0 ), -1 );
//ds triangulations - set on success else null
const CPoint3DCAMERA* vecPointTriangulatedSTEREOSVDLS( 0 );
const CPoint3DCAMERA* vecPointTriangulatedSTEREOQRLS( 0 );
const CPoint3DCAMERA* vecPointTriangulatedSTEREOEDS( 0 );
const CPoint3DCAMERA* vecPointTriangulatedSTEREOSVDDLT( 0 );
try
{
//ds triangulate the point
vecPointTriangulatedSTEREOSVDLS = new CPoint3DCAMERA( g_pTriangulator->getPointTriangulatedLimitedSVDLS( matDisplayRIGHT, matPreprocessedRIGHT, cKeyPoint, matReferenceDescriptorLEFT ) );
}
catch( const CExceptionNoMatchFound& p_cException )
{
//std::cout << "exception: " << p_cException.what( ) << std::endl;
}
/*try
{
vecPointTriangulatedSTEREOQRLS = new CPoint3DInCameraFrame( g_pTriangulator->getPointTriangulatedLimitedQRLS( matPreprocessedRIGHT, cKeyPoint, matReferenceDescriptorLEFT ) );
}
catch( const CExceptionNoMatchFound& p_cException )
{
}
try
{
vecPointTriangulatedSTEREOSVDDLT = new CPoint3DInCameraFrame( g_pTriangulator->getPointTriangulatedLimitedSVDDLT( matPreprocessedRIGHT, cKeyPoint, matReferenceDescriptorLEFT ) );
}
catch( const CExceptionNoMatchFound& p_cException )
{
}*/
/*try
{
std::vector< cv::KeyPoint > vecKeyPointsEDS;
std::vector< CPoint3DInCameraFrame > vecTriangulatedPoints;
//ds normalized coords
const CPoint2DInCameraFrame vecLandmarkLEFTNormalized( g_pCameraSTEREO->m_pCameraLEFT->getNormalized( ptLandmarkLEFT ) );
//const CPoint2DInCameraFrame vecLandmarkLEFTNormalized( ptLandmarkLEFT.x/g_pCameraSTEREO->m_pCameraLEFT->m_dFx, ptLandmarkLEFT.y/g_pCameraSTEREO->m_pCameraLEFT->m_dFy );*/
/*ds calibrate scaling factor for depth of one meter (EPIPOLAR CONSTRAINT)
double dErrorPixel( 1000.0 );
double dVerticalFactor( 1.0 );
#pragma omp parallel for shared( dErrorPixel, dVerticalFactor )
for( uint32_t u = 1; u < 20; ++u )
{
const double dDepthSampling( u/static_cast< double >( 10.0 ) );
for( int32_t i = 1; i < 15; ++i )
{
//ds sample current solution
const double dVerticalFactorCurrent = i/static_cast< double >( 10.0 );
const CPoint3DInCameraFrame vecPointUncalibratedEDSLEFT( dVerticalFactorCurrent*dDepthSampling*vecLandmarkLEFTNormalized(0), dVerticalFactorCurrent*dDepthSampling*vecLandmarkLEFTNormalized(1), dDepthSampling );
//const CPoint3DInCameraFrame vecPointUncalibratedEDSRIGHT( g_pCameraSTEREO->m_matTransformLEFTtoRIGHT*vecPointUncalibratedEDSLEFT );
const double dErrorPixelCurrent( std::fabs( g_pCameraSTEREO->m_pCameraRIGHT->getProjection( vecPointUncalibratedEDSLEFT ).y-ptLandmarkLEFT.y ) );
if( dErrorPixel > dErrorPixelCurrent )
{
dVerticalFactor = dVerticalFactorCurrent;
dErrorPixel = dErrorPixelCurrent;
}
}
}
assert( 1000.0 != dErrorPixel );
if( 0.1 == dVerticalFactor || 1.5 == dVerticalFactor )
{
throw CExceptionNoMatchFound( "<CTriangulator>(getPointTriangulatedLimited) could not calibrate offset" );
}*/
//std::cout << "best min error: " << dErrorPixel << " (scale factor: " << dVerticalFactor << " )" << std::endl;
//ds exponential depth sampling triangulation EDS
//for( int32_t iExponent = -50; iExponent < 50; ++iExponent )
/*#pragma omp parallel for
for( uint32_t uDepthDecimeter = 1; uDepthDecimeter < 100; ++uDepthDecimeter )
{
//ds current depth
const double dDepthMeter( uDepthDecimeter/static_cast< double >( 10.0 ) );
//ds "compute triangulated point"
const CPoint3DInCameraFrame vecPointTriangulatedEDSLEFT( vecLandmarkLEFTNormalized(0)*dDepthMeter, vecLandmarkLEFTNormalized(1)*dDepthMeter, dDepthMeter );
//const CPoint3DInCameraFrame vecPointTriangulatedEDSRIGHT( g_pCameraSTEREO->m_matTransformLEFTtoRIGHT*vecPointTriangulatedEDSLEFT );
//std::cout << vecPointTriangulatedEDSLEFT.transpose( ) << " - " << vecPointTriangulatedEDSRIGHT.transpose( ) << std::endl;
//ds project the point into the right camera frame
const cv::Point2d ptLandmarkRIGHTEDS( g_pCameraSTEREO->m_pCameraRIGHT->getProjection( vecPointTriangulatedEDSLEFT ) );
if( g_pCameraSTEREO->m_cVisibleRange.contains( ptLandmarkRIGHTEDS ) )
{
//ds TODO shift vector combination
#pragma omp critical
{
vecKeyPointsEDS.push_back( cv::KeyPoint( ptLandmarkRIGHTEDS, cKeyPoint.size ) );
vecTriangulatedPoints.push_back( vecPointTriangulatedEDSLEFT );
}
cv::circle( matDisplayRIGHT, ptLandmarkRIGHTEDS, 2, CColorCodeBGR( 200, 200, 200 ), -1 );
}
else
{
//ds stop condition
if( !vecKeyPointsEDS.empty( ) )
{
//break;
uDepthDecimeter = 100;
}
}
}
if( !vecKeyPointsEDS.empty( ) )
{
//ds compute descriptors for the keypoints
CDescriptor matDescriptorsEDS;
g_pExtractor->compute( matPreprocessedRIGHT, vecKeyPointsEDS, matDescriptorsEDS );
//ds match the descriptors
std::vector< cv::DMatch > vecMatches;
g_pMatcher->match( matReferenceDescriptorLEFT, matDescriptorsEDS, vecMatches );
if( vecMatches.empty( ) )
{
throw CExceptionNoMatchFound( "<CTriangulator>(getPointTriangulatedLimited) no match found" );
}
//ds check match quality
if( g_dMatchingDistanceCutoffTriangulation > vecMatches[0].distance )
{
vecPointTriangulatedSTEREOEDS = new CPoint3DInCameraFrame( vecTriangulatedPoints[vecMatches[0].trainIdx] );
}
else
{
throw CExceptionNoMatchFound( "<CTriangulator>(getPointTriangulatedLimited) matching distance: " + std::to_string( vecMatches[0].distance ) );
}
}
}
catch( const CExceptionNoMatchFound& p_cException )
{
}*/
//ds escape if no depth has been computed
if( 0 == vecPointTriangulatedSTEREOEDS && 0 == vecPointTriangulatedSTEREOQRLS && 0 == vecPointTriangulatedSTEREOSVDLS && 0 == vecPointTriangulatedSTEREOSVDDLT )
{
//ds UGLY
continue;
}
std::string strDepthInfo( "" );
if( 0 != vecPointTriangulatedSTEREOSVDLS )
{
//ds compute error
const cv::Point2d ptLandmarkRIGHT( g_pCameraSTEREO->m_pCameraRIGHT->getProjection( *vecPointTriangulatedSTEREOSVDLS ) );
const double dDisparityPixels( ptLandmarkLEFT.x-ptLandmarkRIGHT.x );
const double dDepthMeters( ( *vecPointTriangulatedSTEREOSVDLS )(2) );
const double dDepthErrorMeters( ( *vecPointTriangulatedSTEREOSVDLS )(2)*( *vecPointTriangulatedSTEREOSVDLS )(2)/( g_pCameraSTEREO->m_dBaselineMeters*g_pCameraSTEREO->m_pCameraLEFT->m_dFx )*dDisparityPixels );
dAverageErrorSVDLS += dDepthErrorMeters/dDepthMeters;
++uPointsSVDDLS;
char chBufferDepth[20];
std::snprintf( chBufferDepth, 20, "|%.2f", ( *vecPointTriangulatedSTEREOSVDLS )(2) );
strDepthInfo += chBufferDepth;
cv::circle( matDisplayRIGHT, ptLandmarkRIGHT, 10, CColorCodeBGR( 255, 0, 255 ), 1 );
}
else
{
strDepthInfo += "|X";
}
/*if( 0 != vecPointTriangulatedSTEREOQRLS )
{
//ds compute error
const cv::Point2d ptLandmarkRIGHT( g_pCameraSTEREO->m_pCameraRIGHT->getProjection(*vecPointTriangulatedSTEREOQRLS ) );
const double dDisparityPixels( ptLandmarkLEFT.x-ptLandmarkRIGHT.x );
const double dDepthMeters( ( *vecPointTriangulatedSTEREOQRLS )(2) );
const double dDepthErrorMeters( ( *vecPointTriangulatedSTEREOQRLS )(2)*( *vecPointTriangulatedSTEREOQRLS )(2)/g_pCameraSTEREO->m_dBaselineMeters*dDisparityPixels/g_pCameraSTEREO->m_pCameraLEFT->m_dFx );
dAverageErrorQRLS += dDepthErrorMeters/dDepthMeters;
++uPointsQRLS;
char chBufferDepth[20];
std::snprintf( chBufferDepth, 20, "|%.2f", ( *vecPointTriangulatedSTEREOQRLS )(2) );
strDepthInfo += chBufferDepth;
cv::circle( matDisplayRIGHT, ptLandmarkRIGHT, 12, CColorCodeBGR( 128, 0, 128 ), 1 );
}
else
{
strDepthInfo += "|X";
}
if( 0 != vecPointTriangulatedSTEREOSVDDLT )
{
//ds compute error
const cv::Point2d ptLandmarkRIGHT( g_pCameraSTEREO->m_pCameraRIGHT->getProjection( *vecPointTriangulatedSTEREOSVDDLT ) );
const double dDisparityPixels( ptLandmarkLEFT.x-ptLandmarkRIGHT.x );
const double dDepthMeters( ( *vecPointTriangulatedSTEREOSVDDLT )(2) );
const double dDepthErrorMeters( ( *vecPointTriangulatedSTEREOSVDDLT )(2)*( *vecPointTriangulatedSTEREOSVDDLT )(2)/( g_pCameraSTEREO->m_dBaselineMeters*g_pCameraSTEREO->m_pCameraLEFT->m_dFx )*dDisparityPixels );
dAverageErrorSVDDLT += dDepthErrorMeters/dDepthMeters;
++uPointsSVDDLT;
char chBufferDepth[20];
std::snprintf( chBufferDepth, 20, "|%.2f", ( *vecPointTriangulatedSTEREOSVDDLT )(2) );
strDepthInfo += chBufferDepth;
cv::circle( matDisplayRIGHT, ptLandmarkRIGHT, 14, CColorCodeBGR( 255, 0, 0 ), 1 );
}
else
{
strDepthInfo += "|X";
}
if( 0 != vecPointTriangulatedSTEREOEDS )
{
//ds compute error
const cv::Point2d ptLandmarkRIGHT( g_pCameraSTEREO->m_pCameraRIGHT->getProjection( *vecPointTriangulatedSTEREOEDS ) );
const double dDisparityPixels( ptLandmarkLEFT.x-ptLandmarkRIGHT.x );
const double dDepthMeters( ( *vecPointTriangulatedSTEREOEDS )(2) );
const double dDepthErrorMeters( dDepthMeters*dDepthMeters/( g_pCameraSTEREO->m_dBaselineMeters*g_pCameraSTEREO->m_pCameraLEFT->m_dFx )*dDisparityPixels );
dAverageErrorEDS += dDepthErrorMeters/dDepthMeters;
++uPointsEDS;
char chBufferDepth[20];
std::snprintf( chBufferDepth, 20, "|%.2f", ( *vecPointTriangulatedSTEREOEDS )(2) );
strDepthInfo += chBufferDepth;
cv::circle( matDisplayRIGHT, ptLandmarkRIGHT, 16, CColorCodeBGR( 0, 255, 0 ), 1 );
}
else
{
strDepthInfo += "|X";
}*/
cv::putText( matDisplayLEFT, strDepthInfo, cv::Point2d( ptLandmarkLEFT.x+cKeyPoint.size, ptLandmarkLEFT.y+cKeyPoint.size ), cv::FONT_HERSHEY_PLAIN, 0.75, CColorCodeBGR( 0, 0, 255 ) );
++uActivePoints;
//ds free memory
if( 0 != vecPointTriangulatedSTEREOSVDLS ){ delete vecPointTriangulatedSTEREOSVDLS; }
if( 0 != vecPointTriangulatedSTEREOQRLS ){ delete vecPointTriangulatedSTEREOQRLS; }
if( 0 != vecPointTriangulatedSTEREOSVDDLT ){ delete vecPointTriangulatedSTEREOSVDDLT; }
if( 0 != vecPointTriangulatedSTEREOEDS ){ delete vecPointTriangulatedSTEREOEDS; }
}
//ds compute averages
if( 0 != uPointsSVDDLS ){ dAverageErrorSVDLS = dAverageErrorSVDLS/uPointsSVDDLS; }
if( 0 != uPointsQRLS ){ dAverageErrorQRLS = dAverageErrorQRLS/uPointsQRLS; }
if( 0 != uPointsSVDDLT ){ dAverageErrorSVDDLT = dAverageErrorSVDDLT/uPointsSVDDLT; }
if( 0 != uPointsEDS ){ dAverageErrorEDS = dAverageErrorEDS/uPointsEDS; }
//ds build display mat
cv::hconcat( matDisplayLEFT, matDisplayRIGHT, g_matDisplay );
char chBuffer[100];
std::snprintf( chBuffer, 100, "ACTIVE POINTS: %lu ERRORS[ SVDLS: %4.2f(%2lu) QRLS: %4.2f(%2lu) SVDDLT: %4.2f(%2lu) EDS: %4.2f(%2lu) ]", uActivePoints,
dAverageErrorSVDLS, uPointsSVDDLS, dAverageErrorQRLS, uPointsQRLS, dAverageErrorSVDDLT, uPointsSVDDLT, dAverageErrorEDS, uPointsEDS );
g_matDisplay( cv::Rect( 0, 0, g_pCameraSTEREO->m_uPixelWidth, 17 ) ).setTo( CColorCodeBGR( 0, 0, 0 ) );
cv::putText( g_matDisplay, chBuffer , cv::Point2i( 2, 12 ), cv::FONT_HERSHEY_PLAIN, 0.8, CColorCodeBGR( 0, 0, 255 ) );
//ds display
cv::imshow( "stereo depth calibration", g_matDisplay );
cv::waitKey( 1 );
}
