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 ); }