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