diff --git a/CMakeLists.txt b/CMakeLists.txt index ae41cf8..d3f55a4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,7 +137,7 @@ add_executable(stereo_fps_kitti src/runnable/stereo_fps_kitti.cpp src/core/CTrac add_executable(republisher_node src/runnable/republisher_node.cpp) add_executable(republisher_kitti src/runnable/republisher_kitti.cpp) add_executable(create_trajectory_kitti src/streaming/create_trajectory_kitti.cpp src/configuration/CConfigurationCameraKITTI.cpp) -add_executable(evaluate_trajectory src/runnable/evaluate_trajectory.cpp src/utility/CLogger.cpp) +add_executable(evaluate_trajectory src/runnable/evaluate_trajectory.cpp src/configuration/CConfigurationCameraKITTI.cpp src/utility/CLogger.cpp) #ds link the binaries against catkin target_link_libraries(message_dumper_node ${catkin_LIBRARIES}) diff --git a/src/core/CTrackerStereoMotionModel.cpp b/src/core/CTrackerStereoMotionModel.cpp index 200acf2..fd87d3c 100644 --- a/src/core/CTrackerStereoMotionModel.cpp +++ b/src/core/CTrackerStereoMotionModel.cpp @@ -214,7 +214,7 @@ void CTrackerStereoMotionModel::finalize( ) if( 1 < m_vecKeyFrames->size( ) ) { //ds landmark start id - const std::vector< CLandmark* >::size_type uIDBeginLandmark = m_vecKeyFrames->at( m_uIDProcessedKeyFrameLAST-1 )->vecMeasurements.front( )->uID; + const std::vector< CLandmark* >::size_type uIDBeginLandmark = m_vecKeyFrames->at( std::max( static_cast< UIDKeyFrame >( 0 ), m_uIDProcessedKeyFrameLAST-1 ) )->vecMeasurements.front( )->uID; //ds newly closed keyframes std::vector< CKeyFrame* > vecClosedKeyFrames( 0 ); diff --git a/src/core/CTrackerStereoMotionModel.h b/src/core/CTrackerStereoMotionModel.h index 49e4aa2..99d1872 100644 --- a/src/core/CTrackerStereoMotionModel.h +++ b/src/core/CTrackerStereoMotionModel.h @@ -132,6 +132,7 @@ class CTrackerStereoMotionModel void finalize( ); void sanitizeFiletree( ){ m_cGraphOptimizer.clearFiles( ); } const double getDistanceTraveled( ) const { return m_dDistanceTraveledMeters; } + const double getTotalDurationOptimizationSeconds( ) const { return m_cGraphOptimizer.getTotalOptimizationDurationSeconds( ); } //ds helpers private: diff --git a/src/core/CTrackerStereoMotionModelKITTI.cpp b/src/core/CTrackerStereoMotionModelKITTI.cpp index 37ed6d0..f922ec9 100644 --- a/src/core/CTrackerStereoMotionModelKITTI.cpp +++ b/src/core/CTrackerStereoMotionModelKITTI.cpp @@ -78,6 +78,9 @@ CTrackerStereoMotionModelKITTI::CTrackerStereoMotionModelKITTI( const EPlaybackM std::printf( "[%06lu](CTrackerStereoMotionModelKITTI) minimum matches for loop closure: %lu\n", m_uFrameCount, m_uMinimumNumberOfMatchesLoopClosure ); std::printf( "[%06lu](CTrackerStereoMotionModelKITTI) instance allocated\n", m_uFrameCount ); CLogger::closeBox( ); + + //ds sanity check + assert( m_uMinimumLandmarksForKeyFrame >= m_uMinimumNumberOfMatchesLoopClosure ); } CTrackerStereoMotionModelKITTI::~CTrackerStereoMotionModelKITTI( ) @@ -101,7 +104,7 @@ CTrackerStereoMotionModelKITTI::~CTrackerStereoMotionModelKITTI( ) for( const CKeyFrame* pKeyFrame: *m_vecKeyFrames ) { //ds log trajectory for benchmarking - CLogger::CLogTrajectory::addEntry( pKeyFrame->uFrameOfCreation, pKeyFrame->matTransformationLEFTtoWORLD.translation( ), Eigen::Quaterniond( pKeyFrame->matTransformationLEFTtoWORLD.linear( ) ) ); + CLogger::CLogTrajectoryKITTI::addEntry( pKeyFrame->uFrameOfCreation, pKeyFrame->matTransformationLEFTtoWORLD, m_vecTranslationToG2o ); delete pKeyFrame; } @@ -109,8 +112,9 @@ CTrackerStereoMotionModelKITTI::~CTrackerStereoMotionModelKITTI( ) /*ds close loggers CLogger::CLogLandmarkCreation::close( ); CLogger::CLogLandmarkFinal::close( ); - CLogger::CLogLandmarkFinalOptimized::close( );*/ - CLogger::CLogTrajectory::close( ); + CLogger::CLogLandmarkFinalOptimized::close( ); + CLogger::CLogTrajectory::close( );*/ + CLogger::CLogTrajectoryKITTI::close( ); std::printf( "[%06lu](~CTrackerStereoMotionModelKITTI) instance deallocated\n", m_uFrameCount ); } @@ -123,8 +127,8 @@ void CTrackerStereoMotionModelKITTI::receivevDataVI( const std::shared_ptr< txt_ cv::Mat matPreprocessedRIGHT( p_pImageRIGHT->image( ) ); //ds preprocessing - cv::equalizeHist( p_pImageLEFT->image( ), matPreprocessedLEFT ); - cv::equalizeHist( p_pImageRIGHT->image( ), matPreprocessedRIGHT ); + //cv::equalizeHist( p_pImageLEFT->image( ), matPreprocessedLEFT ); + //cv::equalizeHist( p_pImageRIGHT->image( ), matPreprocessedRIGHT ); //ds current timestamp const double dTimestampSeconds = p_pImageLEFT->timestamp( ); @@ -177,7 +181,7 @@ void CTrackerStereoMotionModelKITTI::finalize( ) if( 1 < m_vecKeyFrames->size( ) ) { //ds landmark start id - const std::vector< CLandmark* >::size_type uIDBeginLandmark = m_vecKeyFrames->at( m_uIDProcessedKeyFrameLAST-1 )->vecMeasurements.front( )->uID; + const std::vector< CLandmark* >::size_type uIDBeginLandmark = m_vecKeyFrames->at( std::max( static_cast< UIDKeyFrame >( 0 ), m_uIDProcessedKeyFrameLAST-1 ) )->vecMeasurements.front( )->uID; //ds newly closed keyframes std::vector< CKeyFrame* > vecClosedKeyFrames( 0 ); @@ -188,39 +192,44 @@ void CTrackerStereoMotionModelKITTI::finalize( ) //ds try to find loop closures for the last 10 keyframes for( std::vector< CKeyFrame* >::const_iterator itKeyFrameForClosure = m_vecKeyFrames->end( )-20; itKeyFrameForClosure < m_vecKeyFrames->end( ); ++itKeyFrameForClosure ) { + //ds buffer key frame const CKeyFrame* pKeyFrameForClosure = *itKeyFrameForClosure; assert( 0 != pKeyFrameForClosure ); - std::printf( "[%06lu](finalize) checking closures for keyframe: %06lu\n", m_uFrameCount, pKeyFrameForClosure->uID ); + //ds only check if we dont have closures for this key frame already + if( pKeyFrameForClosure->vecLoopClosures.empty( ) ) + { + std::printf( "[%06lu](finalize) checking closures for keyframe: %06lu\n", m_uFrameCount, pKeyFrameForClosure->uID ); - //ds detect loop closures (practically unlimited radius) - const std::vector< const CKeyFrame::CMatchICP* > vecLoopClosures( _getLoopClosuresForKeyFrame( pKeyFrameForClosure->uID, - pKeyFrameForClosure->matTransformationLEFTtoWORLD, - pKeyFrameForClosure->vecCloud, - 1e12, - 10 ) ); + //ds detect loop closures (practically unlimited radius) + const std::vector< const CKeyFrame::CMatchICP* > vecLoopClosures( _getLoopClosuresForKeyFrame( pKeyFrameForClosure->uID, + pKeyFrameForClosure->matTransformationLEFTtoWORLD, + pKeyFrameForClosure->vecCloud, + 1e12, + 10 ) ); - std::printf( "[%06lu](finalize) found closures: %lu\n", m_uFrameCount, vecLoopClosures.size( ) ); + std::printf( "[%06lu](finalize) found closures: %lu\n", m_uFrameCount, vecLoopClosures.size( ) ); - //ds if we found closures - if( !vecLoopClosures.empty( ) ) - { - //ds first allocate - CKeyFrame* pClosedKeyFrame = new CKeyFrame( pKeyFrameForClosure->uID, - pKeyFrameForClosure->uFrameOfCreation, - pKeyFrameForClosure->matTransformationLEFTtoWORLD, - pKeyFrameForClosure->vecLinearAccelerationNormalized, - pKeyFrameForClosure->vecMeasurements, - pKeyFrameForClosure->vecCloud, - vecLoopClosures, - pKeyFrameForClosure->dInformationFactor ); - - assert( 0 != pClosedKeyFrame ); - - //ds create new frame - vecClosedKeyFrames.push_back( pClosedKeyFrame ); - - std::printf( "[%06lu](finalize) closed keyframe: %06lu\n", m_uFrameCount, pKeyFrameForClosure->uID ); + //ds if we found closures + if( !vecLoopClosures.empty( ) ) + { + //ds first allocate + CKeyFrame* pClosedKeyFrame = new CKeyFrame( pKeyFrameForClosure->uID, + pKeyFrameForClosure->uFrameOfCreation, + pKeyFrameForClosure->matTransformationLEFTtoWORLD, + pKeyFrameForClosure->vecLinearAccelerationNormalized, + pKeyFrameForClosure->vecMeasurements, + pKeyFrameForClosure->vecCloud, + vecLoopClosures, + pKeyFrameForClosure->dInformationFactor ); + + assert( 0 != pClosedKeyFrame ); + + //ds create new frame + vecClosedKeyFrames.push_back( pClosedKeyFrame ); + + std::printf( "[%06lu](finalize) closed keyframe: %06lu\n", m_uFrameCount, pKeyFrameForClosure->uID ); + } } } @@ -243,13 +252,14 @@ void CTrackerStereoMotionModelKITTI::finalize( ) //ds check if we have to update closures in the graph if( m_uIDProcessedKeyFrameLAST > vecClosedKeyFrames.front( )->uID ) { - std::printf( "[%06lu](finalize) updating already optimized keyframes", m_uFrameCount ); + std::printf( "[%06lu](finalize) updating already optimized keyframes \n", m_uFrameCount ); //ds update the already added closures m_cGraphOptimizer.updateLoopClosuresFromKeyFrame( vecClosedKeyFrames.front( )->uID, m_uIDProcessedKeyFrameLAST, m_vecTranslationToG2o ); - - std::printf( "complete\n" ); } + + //ds lock initial trajectory + //m_cGraphOptimizer.lockTrajectory( 0, 100 ); } } diff --git a/src/core/CTrackerStereoMotionModelKITTI.h b/src/core/CTrackerStereoMotionModelKITTI.h index 198a2e5..eadd157 100644 --- a/src/core/CTrackerStereoMotionModelKITTI.h +++ b/src/core/CTrackerStereoMotionModelKITTI.h @@ -60,7 +60,7 @@ class CTrackerStereoMotionModelKITTI const uint8_t m_uVisibleLandmarksMinimum; const double m_dMinimumDepthMeters = 0.05; const double m_dMaximumDepthMeters = 1000.0; - const UIDFrame m_uMaximumNumberOfFramesWithoutDetection = 5; //20; + const UIDFrame m_uMaximumNumberOfFramesWithoutDetection = 10; //20; UIDFrame m_uNumberOfFramesWithoutDetection = 0; std::shared_ptr< CTriangulator > m_pTriangulator; @@ -82,12 +82,12 @@ class CTrackerStereoMotionModelKITTI Eigen::Vector3d m_vecTranslationToG2o; //ds loop closing - const UIDKeyFrame m_uMinimumLoopClosingKeyFrameDistance = 20; //20 - const UIDLandmark m_uMinimumNumberOfMatchesLoopClosure = 25; //25 + const UIDKeyFrame m_uMinimumLoopClosingKeyFrameDistance = 50; //20 + const UIDLandmark m_uMinimumNumberOfMatchesLoopClosure = 20; //25 const std::vector< CKeyFrame* >::size_type m_uLoopClosingKeyFrameWaitingQueue = 1; std::vector< CKeyFrame* >::size_type m_uLoopClosingKeyFramesInQueue = 0; UIDKeyFrame m_uIDLoopClosureOptimizedLAST = 0; - const double m_dLoopClosingRadiusSquaredMeters = 1000.0; + const double m_dLoopClosingRadiusSquaredMeters = 10000.0; //uint32_t m_uOptimizationsWithoutLoopClosure = 0; //double m_dOptimizationsWithoutLoopClosureDistanceMeters = 0.0; @@ -126,6 +126,7 @@ class CTrackerStereoMotionModelKITTI void finalize( ); void sanitizeFiletree( ){ m_cGraphOptimizer.clearFiles( ); } const double getDistanceTraveled( ) const { return m_dDistanceTraveledMeters; } + const double getTotalDurationOptimizationSeconds( ) const { return m_cGraphOptimizer.getTotalOptimizationDurationSeconds( ); } //ds helpers private: diff --git a/src/optimization/Cg2oOptimizer.cpp b/src/optimization/Cg2oOptimizer.cpp index 927ae78..f95ddb7 100644 --- a/src/optimization/Cg2oOptimizer.cpp +++ b/src/optimization/Cg2oOptimizer.cpp @@ -22,20 +22,29 @@ Cg2oOptimizer::Cg2oOptimizer( const std::shared_ptr< CStereoCamera > p_pCameraST CLogger::openBox( ); std::printf( "(Cg2oOptimizer) configuration: lm_var_cholmod COMPLETE\n" ); + m_uIDKeyFrameFrom = -1; + + //ds trajectory only + m_pVertexPoseFIRSTNOTINGRAPH = new g2o::VertexSE3( ); + m_pVertexPoseFIRSTNOTINGRAPH->setEstimate( p_matTransformationLEFTtoWORLDInitial ); + m_pVertexPoseFIRSTNOTINGRAPH->setId( m_uIDKeyFrameFrom+1 ); + m_pVertexPoseFIRSTNOTINGRAPH->setFixed( true ); + m_cOptimizerSparseTrajectoryOnly.addVertex( m_pVertexPoseFIRSTNOTINGRAPH ); + //ds add the first pose separately - m_pVertexPoseLAST = new g2o::VertexSE3( ); - m_pVertexPoseLAST->setEstimate( p_matTransformationLEFTtoWORLDInitial ); - m_pVertexPoseLAST->setId( m_uIDShift-1 ); - m_pVertexPoseLAST->setFixed( true ); + m_pVertexPoseFIRSTNOTINGRAPH = new g2o::VertexSE3( ); + m_pVertexPoseFIRSTNOTINGRAPH->setEstimate( p_matTransformationLEFTtoWORLDInitial ); + m_pVertexPoseFIRSTNOTINGRAPH->setId( m_uIDKeyFrameFrom+m_uIDShift ); + m_pVertexPoseFIRSTNOTINGRAPH->setFixed( true ); //ds set to graph - m_cOptimizerSparse.addVertex( m_pVertexPoseLAST ); - m_cOptimizerSparse.addEdge( _getEdgeLinearAcceleration( m_pVertexPoseLAST, Eigen::Vector3d( 0.0, -1.0, 0.0 ) ) ); + m_cOptimizerSparse.addVertex( m_pVertexPoseFIRSTNOTINGRAPH ); + m_cOptimizerSparse.addEdge( _getEdgeLinearAcceleration( m_pVertexPoseFIRSTNOTINGRAPH, Eigen::Vector3d( 0.0, -1.0, 0.0 ) ) ); //ds get a copy m_pVertexPoseFIRSTNOTINGRAPH = new g2o::VertexSE3( ); m_pVertexPoseFIRSTNOTINGRAPH->setEstimate( p_matTransformationLEFTtoWORLDInitial ); - m_pVertexPoseFIRSTNOTINGRAPH->setId( m_uIDShift-1 ); + m_pVertexPoseFIRSTNOTINGRAPH->setId( m_uIDKeyFrameFrom+m_uIDShift ); m_pVertexPoseFIRSTNOTINGRAPH->setFixed( true ); //std::printf( "(Cg2oOptimizer) configuration: gn_var_cholmod\n" ); @@ -51,11 +60,12 @@ Cg2oOptimizer::Cg2oOptimizer( const std::shared_ptr< CStereoCamera > p_pCameraST m_vecKeyFrames( p_vecKeyFrames ), m_matInformationPose( 100000*Eigen::Matrix< double, 6, 6 >::Identity( ) ), m_matInformationLoopClosure( 10*m_matInformationPose ), - m_matInformationLandmarkClosure( 1000*Eigen::Matrix< double, 3, 3 >::Identity( ) ) + m_matInformationLandmarkClosure( Eigen::Matrix< double, 3, 3 >::Identity( ) ) { m_vecLandmarksInGraph.clear( ); m_vecKeyFramesInGraph.clear( ); m_vecPoseEdges.clear( ); + m_mapEdgeIDs.clear( ); //ds configure the solver (var_cholmod) //m_cOptimizerSparse.setVerbose( true ); @@ -65,6 +75,13 @@ Cg2oOptimizer::Cg2oOptimizer( const std::shared_ptr< CStereoCamera > p_pCameraST g2o::OptimizationAlgorithmLevenberg* pAlgorithm( new g2o::OptimizationAlgorithmLevenberg( pSolver ) ); m_cOptimizerSparse.setAlgorithm( pAlgorithm ); + //ds trajectory only + g2o::BlockSolver_6_3::LinearSolverType* pLinearSolverTrajectoryOnly( new g2o::LinearSolverCholmod< g2o::BlockSolver_6_3::PoseMatrixType >( ) ); + g2o::BlockSolver_6_3* pSolverTrajectoryOnly( new g2o::BlockSolver_6_3( pLinearSolverTrajectoryOnly ) ); + //g2o::OptimizationAlgorithmGaussNewton* pAlgorithmTrajectoryOnly( new g2o::OptimizationAlgorithmGaussNewton( pSolverTrajectoryOnly ) ); + g2o::OptimizationAlgorithmLevenberg* pAlgorithmTrajectoryOnly( new g2o::OptimizationAlgorithmLevenberg( pSolverTrajectoryOnly ) ); + m_cOptimizerSparseTrajectoryOnly.setAlgorithm( pAlgorithmTrajectoryOnly ); + //ds set world g2o::ParameterSE3Offset* pOffsetWorld = new g2o::ParameterSE3Offset( ); pOffsetWorld->setOffset( Eigen::Isometry3d::Identity( ) ); @@ -104,7 +121,10 @@ Cg2oOptimizer::~Cg2oOptimizer( ) void Cg2oOptimizer::optimizeTailLoopClosuresOnly( const UIDKeyFrame& p_uIDBeginKeyFrame, const Eigen::Vector3d& p_vecTranslationToG2o ) { - //ds clear nodes and edges + //ds disabled for now + assert( false ); + + /*ds clear nodes and edges m_cOptimizerSparse.clear( ); //ds local optimization chunks: +1 at the end because std::vector[begin,end) @@ -166,7 +186,7 @@ void Cg2oOptimizer::optimizeTailLoopClosuresOnly( const UIDKeyFrame& p_uIDBeginK //ds done std::printf( "(optimizeTailLoopClosuresOnly) optimized poses: %lu (error final: %f)\n", vecChunkKeyFrames.size( ), m_cOptimizerSparse.activeChi2( ) ); - ++m_uOptimizations; + ++m_uOptimizations;*/ } void Cg2oOptimizer::optimizeTail( const UIDKeyFrame& p_uIDBeginKeyFrame ) @@ -269,11 +289,10 @@ void Cg2oOptimizer::optimizeContinuous( const UIDFrame& p_uFrame, const std::vector< CKeyFrame* >::size_type& p_uLoopClosureKeyFrames ) { //ds local optimization chunks - assert( m_vecKeyFrames->begin( )+p_uIDBeginKeyFrame < m_vecKeyFrames->end( ) ); + assert( m_vecKeyFrames->begin( )+p_uIDBeginKeyFrame <= m_vecKeyFrames->end( ) ); const std::vector< CKeyFrame* > vecChunkKeyFrames( m_vecKeyFrames->begin( )+p_uIDBeginKeyFrame, m_vecKeyFrames->end( ) ); - assert( !vecChunkKeyFrames.empty( ) ); + if( vecChunkKeyFrames.empty( ) ){ return; } assert( !vecChunkKeyFrames.front( )->vecMeasurements.empty( ) ); - //std::printf( "[%06lu](optimizeContinuous) loading landmarks (total duration: %.2fs)\n", p_uFrame, CLogger::getTimeSeconds( )-dTimeStartSeconds ); //ds sane input ++m_uOptimizations; @@ -283,13 +302,15 @@ void Cg2oOptimizer::optimizeContinuous( const UIDFrame& p_uFrame, { //ds closure count uint32_t uLoopClosuresTotal = 0; - uint32_t uLoopClosuresGood = 0; + + //ds loop closure holder + std::vector< CLoopClosureRaw > vecLoopClosures; //ds loop over the camera vertices vector for( CKeyFrame* pKeyFrame: vecChunkKeyFrames ) { //ds add current camera pose - g2o::VertexSE3* pVertexPoseCurrent = _setAndgetPose( m_pVertexPoseLAST, pKeyFrame, p_vecTranslationToG2o ); + g2o::VertexSE3* pVertexPoseCurrent = _setAndgetPose( m_uIDKeyFrameFrom, pKeyFrame, p_vecTranslationToG2o ); //ds closed edges OptimizableGraph::EdgeSet vecLoopClosureEdges; @@ -300,13 +321,23 @@ void Cg2oOptimizer::optimizeContinuous( const UIDFrame& p_uFrame, //ds accumulate the elements vecLoopClosureEdges.insert( _getEdgeLoopClosure( pVertexPoseCurrent, pKeyFrame, pClosure ) ); ++uLoopClosuresTotal; + + //ds get loop closure + //g2o::EdgeSE3* pEdgeLoopClosure = _getEdgeLoopClosure( pVertexPoseCurrent, pKeyFrame, pClosure ); + + //ds add to graph + //m_cOptimizerSparseTrajectoryOnly.addEdge( pEdgeLoopClosure ); + //vecLoopClosures.push_back( CLoopClosureRaw( pClosure->pKeyFrameReference->uID, pKeyFrame->uID, pEdgeLoopClosure ) ); } //ds if there were any loop closures for this keyframe if( !vecLoopClosureEdges.empty( ) ) { + const g2o::HyperGraph::VertexIDMap::iterator itPoseCurrent( m_cOptimizerSparseTrajectoryOnly.vertices( ).find( pKeyFrame->uID+1 ) ); + assert( m_cOptimizerSparseTrajectoryOnly.vertices( ).end( ) != itPoseCurrent ); + m_cBufferClosures.addEdgeSet( vecLoopClosureEdges ); - m_cBufferClosures.addVertex( pVertexPoseCurrent ); + m_cBufferClosures.addVertex( dynamic_cast< g2o::VertexSE3* >( itPoseCurrent->second ) ); } //ds evaluate loop closure window @@ -329,11 +360,11 @@ void Cg2oOptimizer::optimizeContinuous( const UIDFrame& p_uFrame, if( m_dMaximumThresholdLoopClosing > itEdgeLoopClosure->second ) { - std::printf( "ADDING CLOSURE %06li to %06li WITH CHI2: %f\n", pEdgeLoopClosure->vertices( )[0]->id( )-m_uIDShift, pEdgeLoopClosure->vertices( )[1]->id( )-m_uIDShift, itEdgeLoopClosure->second ); + std::printf( "ADDING CLOSURE %06i to %06i WITH CHI2: %f\n", pEdgeLoopClosure->vertices( )[0]->id( )-1, pEdgeLoopClosure->vertices( )[1]->id( )-1, itEdgeLoopClosure->second ); //ds add to graph - m_cOptimizerSparse.addEdge( pEdgeLoopClosure ); - ++uLoopClosuresGood; + m_cOptimizerSparseTrajectoryOnly.addEdge( pEdgeLoopClosure ); + vecLoopClosures.push_back( CLoopClosureRaw( pEdgeLoopClosure->vertices( )[0]->id( )-1, pEdgeLoopClosure->vertices( )[1]->id( )-1, pEdgeLoopClosure ) ); } } @@ -346,45 +377,71 @@ void Cg2oOptimizer::optimizeContinuous( const UIDFrame& p_uFrame, m_cBufferClosures.updateList( p_uLoopClosureKeyFrames ); //ds update last - m_pVertexPoseLAST = pVertexPoseCurrent; + m_uIDKeyFrameFrom = pKeyFrame->uID; } + //ds we always process all keyframes + m_vecKeyFramesInGraph.insert( m_vecKeyFramesInGraph.end( ), vecChunkKeyFrames.begin( ), vecChunkKeyFrames.end( ) ); + //ds if there were any closures accepted - if( 0 < uLoopClosuresGood ) + if( !vecLoopClosures.empty( ) ) { - std::printf( "[%06lu](optimizeContinuous) optimizing loop closures: %u/%u\n", p_uFrame, uLoopClosuresGood, uLoopClosuresTotal ); + std::printf( "[%06lu](optimizeContinuous) optimizing loop closures: %lu/%u\n", p_uFrame, vecLoopClosures.size( ), uLoopClosuresTotal ); + + /*ds lock selected closures + for( CLoopClosureRaw cEdgeLoopClosureRaw: vecLoopClosures ) + { + const g2o::HyperGraph::VertexIDMap::iterator itReference( m_cOptimizerSparseTrajectoryOnly.vertices( ).find( cEdgeLoopClosureRaw.uIDReference+1 ) ); + assert( itReference != m_cOptimizerSparseTrajectoryOnly.vertices( ).end( ) ); + g2o::VertexSE3* pVertexReference = dynamic_cast< g2o::VertexSE3* >( itReference->second ); + pVertexReference->setFixed( true ); + }*/ //ds save closure situation char chBufferLC[256]; std::snprintf( chBufferLC, 256, "g2o/local/keyframes_%06lu-%06lu", vecChunkKeyFrames.front( )->uID, vecChunkKeyFrames.back( )->uID ); std::string strPrefixLC( chBufferLC ); - m_cOptimizerSparse.save( ( strPrefixLC + "_closed.g2o" ).c_str( ) ); + m_cOptimizerSparseTrajectoryOnly.save( ( strPrefixLC + "_closed.g2o" ).c_str( ) ); //ds initialize optimization - m_cOptimizerSparse.initializeOptimization( ); + m_cOptimizerSparseTrajectoryOnly.initializeOptimization( ); //ds timing const double dTimeStartSecondsLC = CLogger::getTimeSeconds( ); //ds start cascading iteration steps - m_cOptimizerSparse.optimize( 1 ); + m_cOptimizerSparseTrajectoryOnly.optimize( 1 ); uint32_t uIterationsDoneLC = 1; if( 10.0 > ( CLogger::getTimeSeconds( )-dTimeStartSecondsLC ) ) { - m_cOptimizerSparse.optimize( 10 ); + m_cOptimizerSparseTrajectoryOnly.optimize( 10 ); uIterationsDoneLC += 10; if( 20.0 > ( CLogger::getTimeSeconds( )-dTimeStartSecondsLC ) ) { - m_cOptimizerSparse.optimize( 100 ); + m_cOptimizerSparseTrajectoryOnly.optimize( 100 ); uIterationsDoneLC += 100; if( 30.0 > ( CLogger::getTimeSeconds( )-dTimeStartSecondsLC ) ) { - m_cOptimizerSparse.optimize( 1000 ); + m_cOptimizerSparseTrajectoryOnly.optimize( 1000 ); uIterationsDoneLC += 1000; } } } + /*ds unlock added closures + for( CLoopClosureRaw cEdgeLoopClosureRaw: vecLoopClosures ) + { + //ds relax locked reference vertices again + const g2o::HyperGraph::VertexIDMap::iterator itReference( m_cOptimizerSparseTrajectoryOnly.vertices( ).find( cEdgeLoopClosureRaw.uIDReference+1 ) ); + assert( itReference != m_cOptimizerSparseTrajectoryOnly.vertices( ).end( ) ); + g2o::VertexSE3* pVertexReference = dynamic_cast< g2o::VertexSE3* >( itReference->second ); + pVertexReference->setFixed( false ); + }*/ + + //ds back propagate the trajectory only graph into the full one + _backPropagateTrajectoryToFull( vecLoopClosures ); + + m_cOptimizerSparseTrajectoryOnly.save( ( strPrefixLC + "_closed_optimized.g2o" ).c_str( ) ); std::printf( "[%06lu](optimizeContinuous) optimization complete (total duration: %.2fs | iterations: %u)\n", p_uFrame, ( CLogger::getTimeSeconds( )-dTimeStartSecondsLC ), uIterationsDoneLC ); } @@ -422,6 +479,55 @@ void Cg2oOptimizer::optimizeContinuous( const UIDFrame& p_uFrame, //ds set landmark measurements _setLandmarkMeasurementsWORLD( pVertexPoseCurrent, pKeyFrame, uMeasurementsStoredPointXYZ, uMeasurementsStoredUVDepth, uMeasurementsStoredUVDisparity ); + + //ds if the key frame was closed + if( !pKeyFrame->vecLoopClosures.empty( ) ) + { + //ds for each closure + for( const CKeyFrame::CMatchICP* pMatch: pKeyFrame->vecLoopClosures ) + { + //ds connect all closed landmarks + for( const CMatchCloud& cMatch: *pMatch->vecMatches ) + { + //ds find the corresponding landmarks + const g2o::HyperGraph::VertexIDMap::iterator itLandmarkQuery( m_cOptimizerSparse.vertices( ).find( cMatch.cPointQuery.uID ) ); + const g2o::HyperGraph::VertexIDMap::iterator itLandmarkReference( m_cOptimizerSparse.vertices( ).find( cMatch.cPointReference.uID ) ); + + //ds if query is in the graph + if( itLandmarkQuery != m_cOptimizerSparse.vertices( ).end( ) ) + { + //ds consistency + assert( cMatch.cPointReference.uID == m_vecLandmarks->at( cMatch.cPointReference.uID )->uID ); + + //ds reference landmark + g2o::VertexPointXYZ* pVertexLandmarkReference = 0; + + //ds check if the reference landmark is present in the graph + if( itLandmarkReference != m_cOptimizerSparse.vertices( ).end( ) ) + { + //ds get it from the graph + pVertexLandmarkReference = dynamic_cast< g2o::VertexPointXYZ* >( itLandmarkReference->second ); + + //ds fix reference landmark + //pVertexLandmarkReference->setFixed( true ); + + //ds set up the edge + g2o::EdgePointXYZ* pEdgeLandmarkClosure( new g2o::EdgePointXYZ( ) ); + + //ds set viewpoints and measurement + pEdgeLandmarkClosure->setVertex( 0, pVertexLandmarkReference ); + pEdgeLandmarkClosure->setVertex( 1, itLandmarkQuery->second ); + pEdgeLandmarkClosure->setMeasurement( Eigen::Vector3d::Zero( ) ); + pEdgeLandmarkClosure->setInformation( m_matInformationLandmarkClosure ); + pEdgeLandmarkClosure->setRobustKernel( new g2o::RobustKernelCauchy( ) ); + + //ds add to optimizer + m_cOptimizerSparse.addEdge( pEdgeLandmarkClosure ); + } + } + } + } + } } } else @@ -430,7 +536,7 @@ void Cg2oOptimizer::optimizeContinuous( const UIDFrame& p_uFrame, for( CKeyFrame* pKeyFrame: vecChunkKeyFrames ) { //ds add current camera pose - g2o::VertexSE3* pVertexPoseCurrent = _setAndgetPose( m_pVertexPoseLAST, pKeyFrame, p_vecTranslationToG2o ); + g2o::VertexSE3* pVertexPoseCurrent = _setAndgetPose( m_uIDKeyFrameFrom, pKeyFrame, p_vecTranslationToG2o ); //ds always save acceleration data m_cOptimizerSparse.addEdge( _getEdgeLinearAcceleration( pVertexPoseCurrent, pKeyFrame->vecLinearAccelerationNormalized ) ); @@ -439,12 +545,13 @@ void Cg2oOptimizer::optimizeContinuous( const UIDFrame& p_uFrame, _setLandmarkMeasurementsWORLD( pVertexPoseCurrent, pKeyFrame, uMeasurementsStoredPointXYZ, uMeasurementsStoredUVDepth, uMeasurementsStoredUVDisparity ); //ds update last - m_pVertexPoseLAST = pVertexPoseCurrent; + m_uIDKeyFrameFrom = pKeyFrame->uID; } + + //ds we always process all keyframes + m_vecKeyFramesInGraph.insert( m_vecKeyFramesInGraph.end( ), vecChunkKeyFrames.begin( ), vecChunkKeyFrames.end( ) ); } - //ds we always process all keyframes - m_vecKeyFramesInGraph.insert( m_vecKeyFramesInGraph.end( ), vecChunkKeyFrames.begin( ), vecChunkKeyFrames.end( ) ); //std::printf( "- complete!\n" ); /*ds if there were loop closures and we are working in large scale @@ -540,6 +647,10 @@ void Cg2oOptimizer::optimizeContinuous( const UIDFrame& p_uFrame, } else {*/ + + //ds back propagate the full graph into trajectory only + _backPropagateTrajectoryToPure( ); + //ds save optimized situation m_cOptimizerSparse.save( ( strPrefix + "_optimized.g2o" ).c_str( ) ); std::printf( "[%06lu](optimizeContinuous) optimization complete (total duration: %.2fs | iterations: %u)\n", p_uFrame, ( CLogger::getTimeSeconds( )-dTimeStartSeconds ), uIterationsDone ); @@ -547,6 +658,8 @@ void Cg2oOptimizer::optimizeContinuous( const UIDFrame& p_uFrame, //ds done //std::printf( "(optimizeContinuous) optimized poses: %lu and landmarks: %lu (error final: %f)\n", vecChunkKeyFrames.size( ), vecChunkLandmarks.size( ), m_cOptimizerSparse.activeChi2( ) ); + + m_dTotalOptimizationDurationSeconds += ( CLogger::getTimeSeconds( )-dTimeStartSeconds ); } void Cg2oOptimizer::clearFiles( ) const @@ -589,6 +702,7 @@ void Cg2oOptimizer::saveFinalGraph( const UIDFrame& p_uFrame, const Eigen::Vecto assert( !m_vecKeyFramesInGraph.empty( ) ); //ds clear all structures + m_uIDKeyFrameFrom = -1; m_cOptimizerSparse.clear( ); //ds add all the landmarks again @@ -596,9 +710,8 @@ void Cg2oOptimizer::saveFinalGraph( const UIDFrame& p_uFrame, const Eigen::Vecto _loadLandmarksToGraph( 0, p_vecTranslationToG2o ); //ds add first pose - m_pVertexPoseLAST = m_pVertexPoseFIRSTNOTINGRAPH; - m_cOptimizerSparse.addVertex( m_pVertexPoseLAST ); - m_cOptimizerSparse.addEdge( _getEdgeLinearAcceleration( m_pVertexPoseLAST, Eigen::Vector3d( 0.0, -1.0, 0.0 ) ) ); + m_cOptimizerSparse.addVertex( m_pVertexPoseFIRSTNOTINGRAPH ); + m_cOptimizerSparse.addEdge( _getEdgeLinearAcceleration( m_pVertexPoseFIRSTNOTINGRAPH, Eigen::Vector3d( 0.0, -1.0, 0.0 ) ) ); //ds info UIDLandmark uMeasurementsStoredPointXYZ = 0; @@ -614,8 +727,29 @@ void Cg2oOptimizer::saveFinalGraph( const UIDFrame& p_uFrame, const Eigen::Vecto //ds for sane id if( 0 <= pKeyFrame->uID && 2*m_uIDShift > pKeyFrame->uID ) { + //ds compose transformation matrix + Eigen::Isometry3d matTransformationLEFTtoWORLD = pKeyFrame->matTransformationLEFTtoWORLD; + matTransformationLEFTtoWORLD.translation( ) += p_vecTranslationToG2o; + //ds add current camera pose - g2o::VertexSE3* pVertexPoseCurrent = _setAndgetPose( m_pVertexPoseLAST, pKeyFrame, p_vecTranslationToG2o ); + g2o::VertexSE3* pVertexPoseCurrent = new g2o::VertexSE3( ); + pVertexPoseCurrent->setEstimate( matTransformationLEFTtoWORLD ); + pVertexPoseCurrent->setId( pKeyFrame->uID+m_uIDShift ); + m_cOptimizerSparse.addVertex( pVertexPoseCurrent ); + + //ds set up the edge to connect the poses + g2o::EdgeSE3* pEdgePoseFromTo = new g2o::EdgeSE3( ); + + //ds set viewpoints and measurement + const g2o::HyperGraph::VertexIDMap::iterator itPoseFrom = m_cOptimizerSparse.vertices( ).find( m_uIDKeyFrameFrom+m_uIDShift ); + assert( m_cOptimizerSparse.vertices( ).end( ) != itPoseFrom ); + g2o::VertexSE3* pVertexPoseFrom = dynamic_cast< g2o::VertexSE3* >( itPoseFrom->second ); + pEdgePoseFromTo->setVertex( 0, pVertexPoseFrom ); + pEdgePoseFromTo->setVertex( 1, pVertexPoseCurrent ); + pEdgePoseFromTo->setMeasurement( pVertexPoseFrom->estimate( ).inverse( )*pVertexPoseCurrent->estimate( ) ); + + //ds add to optimizer + m_cOptimizerSparse.addEdge( pEdgePoseFromTo ); //ds check if we got loop closures for this frame for( const CKeyFrame::CMatchICP* pClosure: pKeyFrame->vecLoopClosures ) @@ -632,7 +766,7 @@ void Cg2oOptimizer::saveFinalGraph( const UIDFrame& p_uFrame, const Eigen::Vecto _setLandmarkMeasurementsWORLD( pVertexPoseCurrent, pKeyFrame, uMeasurementsStoredPointXYZ, uMeasurementsStoredUVDepth, uMeasurementsStoredUVDisparity ); //ds update last - m_pVertexPoseLAST = pVertexPoseCurrent; + m_uIDKeyFrameFrom = pKeyFrame->uID; } else { @@ -658,9 +792,6 @@ void Cg2oOptimizer::updateLoopClosuresFromKeyFrame( const std::vector< CKeyFrame assert( p_uIDEndKeyFrame <= m_vecKeyFrames->size( ) ); assert( p_uIDEndKeyFrame <= m_vecKeyFramesInGraph.size( ) ); - //ds lowest closed key frame id - std::vector< CKeyFrame* >::size_type uIDLowestKeyFrameClosed = p_uIDBeginKeyFrame; - //ds update key frames in graph (null now) and g2o for( std::vector< CKeyFrame* >::size_type uID = p_uIDBeginKeyFrame; uID < p_uIDEndKeyFrame; ++uID ) { @@ -692,19 +823,28 @@ void Cg2oOptimizer::updateLoopClosuresFromKeyFrame( const std::vector< CKeyFrame assert( 0 != pClosure ); //ds set the closure - _setLoopClosure( pVertex, pKeyFrame, pClosure, p_vecTranslationToG2o ); + //_setLoopClosure( pVertex, pKeyFrame, pClosure, p_vecTranslationToG2o ); - //ds update lowest id if lower - if( uIDLowestKeyFrameClosed > static_cast< std::vector< CKeyFrame* >::size_type >( pClosure->pKeyFrameReference->uID ) ) - { - uIDLowestKeyFrameClosed = pClosure->pKeyFrameReference->uID; - } + //ds get loop closure + g2o::EdgeSE3* pEdgeLoopClosure = _getEdgeLoopClosure( pVertex, pKeyFrame, pClosure ); + + //ds add to graph + m_cOptimizerSparseTrajectoryOnly.addEdge( pEdgeLoopClosure ); } } } +} - //ds lock all poses before lowest - for( std::vector< CKeyFrame* >::size_type uID = uIDLowestKeyFrameClosed; 0 < uID; --uID ) +/*void Cg2oOptimizer::lockTrajectory( const std::vector< CKeyFrame* >::size_type& p_uIDBeginKeyFrame, + const std::vector< CKeyFrame* >::size_type& p_uIDEndKeyFrame ) +{ + assert( p_uIDBeginKeyFrame < p_uIDEndKeyFrame ); + assert( p_uIDBeginKeyFrame < m_vecKeyFrames->size( ) ); + assert( p_uIDEndKeyFrame <= m_vecKeyFrames->size( ) ); + assert( p_uIDEndKeyFrame <= m_vecKeyFramesInGraph.size( ) ); + + //ds lock all poses + for( std::vector< CKeyFrame* >::size_type uID = p_uIDBeginKeyFrame; uID < p_uIDEndKeyFrame; ++uID ) { //ds update g2o: try to retrieve vertex const g2o::HyperGraph::VertexIDMap::iterator itPose( m_cOptimizerSparse.vertices( ).find( uID+m_uIDShift ) ); @@ -718,7 +858,40 @@ void Cg2oOptimizer::updateLoopClosuresFromKeyFrame( const std::vector< CKeyFrame std::cerr << "fixed keyframe: " << uID << std::endl; } -} + + //ds if feasiable + if( 50 < m_vecKeyFramesInGraph.back( )->uID ) + { + //ds lower limit + const std::vector< CKeyFrame* >::size_type uIDStart = m_vecKeyFramesInGraph.back( )->uID-50; + + //ds increase stiffness of recent trajectory + for( std::vector< CKeyFrame* >::size_type uID = m_vecKeyFramesInGraph.back( )->uID; uID > uIDStart; --uID ) + { + //ds update g2o: try to retrieve vertex + const g2o::HyperGraph::VertexIDMap::iterator itPose( m_cOptimizerSparse.vertices( ).find( uID+m_uIDShift ) ); + + //ds if we can update the vertex + assert( itPose != m_cOptimizerSparse.vertices( ).end( ) ); + + //ds extract the pose and lock it + g2o::VertexSE3* pVertex = dynamic_cast< g2o::VertexSE3* >( itPose->second ); + + assert( 0 != pVertex ); + + //ds buffer connecting edge + g2o::EdgeSE3* pEdgePoseFromTo = dynamic_cast< g2o::EdgeSE3* >( *( pVertex->edges( ).begin( ) ) ); + + if( 0 != pEdgePoseFromTo ) + { + //ds increase information matrix + pEdgePoseFromTo->setInformation( static_cast< Eigen::Matrix< double, 6,6 > >( 100*pEdgePoseFromTo->information( ) ) ); + + std::cerr << "strengthened keyframe: " << uID << std::endl; + } + } + } +}*/ g2o::EdgeSE3LinearAcceleration* Cg2oOptimizer::_getEdgeLinearAcceleration( g2o::VertexSE3* p_pVertexPose, const CLinearAccelerationIMU& p_vecLinearAccelerationNormalized ) const @@ -756,7 +929,7 @@ g2o::EdgeSE3PointXYZ* Cg2oOptimizer::_getEdgePointXYZ( g2o::VertexSE3* p_pVertex pEdgePointXYZ->setInformation( g2o::Matrix3D( arrInformationMatrixXYZ ) ); //ds set robust kernel - pEdgePointXYZ->setRobustKernel( new g2o::RobustKernelCauchy( ) ); + //pEdgePointXYZ->setRobustKernel( new g2o::RobustKernelCauchy( ) ); return pEdgePointXYZ; } @@ -780,7 +953,7 @@ g2o::EdgeSE3PointXYZDepth* Cg2oOptimizer::_getEdgeUVDepthLEFT( g2o::VertexSE3* p pEdgeProjectedDepth->setInformation( g2o::Matrix3D( arrInformationMatrixDepth ) ); //ds set robust kernel - pEdgeProjectedDepth->setRobustKernel( new g2o::RobustKernelCauchy( ) ); + //pEdgeProjectedDepth->setRobustKernel( new g2o::RobustKernelCauchy( ) ); return pEdgeProjectedDepth; } @@ -815,7 +988,7 @@ g2o::EdgeSE3PointXYZDisparity* Cg2oOptimizer::_getEdgeUVDisparityLEFT( g2o::Vert g2o::EdgeSE3* Cg2oOptimizer::_getEdgeLoopClosure( g2o::VertexSE3* p_pVertexPoseCurrent, const CKeyFrame* pKeyFrameCurrent, const CKeyFrame::CMatchICP* p_pClosure ) { - //ds find the corresponding pose in the current graph + /*ds find the corresponding pose in the current graph const g2o::HyperGraph::VertexIDMap::iterator itClosure( m_cOptimizerSparse.vertices( ).find( p_pClosure->pKeyFrameReference->uID+m_uIDShift ) ); //ds has to be found @@ -831,7 +1004,25 @@ g2o::EdgeSE3* Cg2oOptimizer::_getEdgeLoopClosure( g2o::VertexSE3* p_pVertexPoseC pEdgeLoopClosure->setVertex( 0, pVertexClosure ); pEdgeLoopClosure->setVertex( 1, p_pVertexPoseCurrent ); pEdgeLoopClosure->setMeasurement( p_pClosure->matTransformationToClosure ); - pEdgeLoopClosure->setInformation( m_matInformationLoopClosure ); + pEdgeLoopClosure->setInformation( m_matInformationLoopClosure );*/ + + //ds find the corresponding poses in the current graph + const g2o::HyperGraph::VertexIDMap::iterator itCurrent( m_cOptimizerSparseTrajectoryOnly.vertices( ).find( pKeyFrameCurrent->uID+1 ) ); + const g2o::HyperGraph::VertexIDMap::iterator itClosure( m_cOptimizerSparseTrajectoryOnly.vertices( ).find( p_pClosure->pKeyFrameReference->uID+1 ) ); + + //ds have to be found + assert( itCurrent != m_cOptimizerSparseTrajectoryOnly.vertices( ).end( ) ); + assert( itClosure != m_cOptimizerSparseTrajectoryOnly.vertices( ).end( ) ); + + //ds set up the edge + g2o::EdgeSE3* pEdgeLoopClosure( new g2o::EdgeSE3( ) ); + + //ds set viewpoints and measurement + pEdgeLoopClosure->setVertex( 0, dynamic_cast< g2o::VertexSE3* >( itClosure->second ) ); + pEdgeLoopClosure->setVertex( 1, dynamic_cast< g2o::VertexSE3* >( itCurrent->second ) ); + pEdgeLoopClosure->setMeasurement( p_pClosure->matTransformationToClosure ); + pEdgeLoopClosure->setInformation( static_cast< Eigen::Matrix< double, 6, 6 > >( 10*pEdgeLoopClosure->information( ) ) ); + pEdgeLoopClosure->setRobustKernel( new g2o::RobustKernelCauchy( ) ); return pEdgeLoopClosure; } @@ -863,7 +1054,7 @@ void Cg2oOptimizer::_loadLandmarksToGraph( const std::vector< CLandmark* >::size const CPoint3DWORLD vecPosition( pLandmark->vecPointXYZOptimized+p_vecTranslationToG2o ); //ds if sane TODO make redundant - if( 1e9 > vecPosition.squaredNorm( ) ) + if( m_dSanePositionThresholdL2 > vecPosition.squaredNorm( ) ) { //ds try to retrieve vertex const g2o::HyperGraph::VertexIDMap::iterator itLandmark( m_cOptimizerSparse.vertices( ).find( pLandmark->uID ) ); @@ -906,7 +1097,7 @@ void Cg2oOptimizer::_loadLandmarksToGraph( const std::vector< CLandmark* >::size //std::printf( "(_loadLandmarksToGraph) landmarks already present in graph: %lu\n", uLandmarksAlreadyInGraph ); } -g2o::VertexSE3* Cg2oOptimizer::_setAndgetPose( g2o::VertexSE3* p_pVertexPoseFrom, CKeyFrame* pKeyFrameCurrent, const Eigen::Vector3d& p_vecTranslationToG2o ) +g2o::VertexSE3* Cg2oOptimizer::_setAndgetPose( std::vector< CKeyFrame* >::size_type& p_uIDKeyFrameFrom, CKeyFrame* pKeyFrameCurrent, const Eigen::Vector3d& p_vecTranslationToG2o ) { //ds compose transformation matrix Eigen::Isometry3d matTransformationLEFTtoWORLD = pKeyFrameCurrent->matTransformationLEFTtoWORLD; @@ -918,13 +1109,22 @@ g2o::VertexSE3* Cg2oOptimizer::_setAndgetPose( g2o::VertexSE3* p_pVertexPoseFrom pVertexPoseCurrent->setId( pKeyFrameCurrent->uID+m_uIDShift ); m_cOptimizerSparse.addVertex( pVertexPoseCurrent ); + //ds for trajectory only as well + g2o::VertexSE3* pVertexPoseCurrentTrajectoryOnly = new g2o::VertexSE3( ); + pVertexPoseCurrentTrajectoryOnly->setEstimate( matTransformationLEFTtoWORLD ); + pVertexPoseCurrentTrajectoryOnly->setId( pKeyFrameCurrent->uID+1 ); + m_cOptimizerSparseTrajectoryOnly.addVertex( pVertexPoseCurrentTrajectoryOnly ); + //ds set up the edge to connect the poses g2o::EdgeSE3* pEdgePoseFromTo = new g2o::EdgeSE3( ); //ds set viewpoints and measurement - pEdgePoseFromTo->setVertex( 0, p_pVertexPoseFrom ); + const g2o::HyperGraph::VertexIDMap::iterator itPoseFrom = m_cOptimizerSparse.vertices( ).find( p_uIDKeyFrameFrom+m_uIDShift ); + assert( m_cOptimizerSparse.vertices( ).end( ) != itPoseFrom ); + g2o::VertexSE3* pVertexPoseFrom = dynamic_cast< g2o::VertexSE3* >( itPoseFrom->second ); + pEdgePoseFromTo->setVertex( 0, pVertexPoseFrom ); pEdgePoseFromTo->setVertex( 1, pVertexPoseCurrent ); - pEdgePoseFromTo->setMeasurement( p_pVertexPoseFrom->estimate( ).inverse( )*pVertexPoseCurrent->estimate( ) ); + pEdgePoseFromTo->setMeasurement( pVertexPoseFrom->estimate( ).inverse( )*pVertexPoseCurrent->estimate( ) ); //ds compute information value based on absolute distance between keyframes const double dInformationFactor = 1.0/(1.0+pEdgePoseFromTo->measurement( ).translation( ).squaredNorm( ) ); @@ -939,8 +1139,21 @@ g2o::VertexSE3* Cg2oOptimizer::_setAndgetPose( g2o::VertexSE3* p_pVertexPoseFrom //ds add to optimizer m_cOptimizerSparse.addEdge( pEdgePoseFromTo ); + //ds find the corresponding pose in the trajectory only graph + const g2o::HyperGraph::VertexIDMap::iterator itPoseFromTrajectoryOnly( m_cOptimizerSparseTrajectoryOnly.vertices( ).find( p_uIDKeyFrameFrom+1 ) ); + assert( m_cOptimizerSparseTrajectoryOnly.vertices( ).end( ) != itPoseFromTrajectoryOnly ); + + //ds trajectory only + g2o::EdgeSE3* pEdgePoseFromToTrajectoryOnly = new g2o::EdgeSE3( ); + pEdgePoseFromToTrajectoryOnly->setVertex( 0, dynamic_cast< g2o::VertexSE3* >( itPoseFromTrajectoryOnly->second ) ); + pEdgePoseFromToTrajectoryOnly->setVertex( 1, pVertexPoseCurrentTrajectoryOnly ); + pEdgePoseFromToTrajectoryOnly->setMeasurement( pEdgePoseFromTo->measurement( ) ); + pEdgePoseFromToTrajectoryOnly->setInformation( static_cast< Eigen::Matrix< double, 6, 6 > >( dInformationFactor*pEdgePoseFromToTrajectoryOnly->information( ) ) ); + m_cOptimizerSparseTrajectoryOnly.addEdge( pEdgePoseFromToTrajectoryOnly ); + //ds add to control structure m_vecPoseEdges.push_back( pEdgePoseFromTo ); + m_mapEdgeIDs.insert( std::make_pair( pKeyFrameCurrent->uID, std::make_pair( pEdgePoseFromTo, pEdgePoseFromToTrajectoryOnly ) ) ); return pVertexPoseCurrent; } @@ -1139,7 +1352,7 @@ void Cg2oOptimizer::_applyOptimization( const UIDFrame& p_uFrame, const std::vec g2o::VertexPointXYZ* pVertex = dynamic_cast< g2o::VertexPointXYZ* >( itLandmark->second ); //ds check if sane - if( 1e9 > pVertex->estimate( ).squaredNorm( ) ) + if( m_dSanePositionThresholdL2 > pVertex->estimate( ).squaredNorm( ) ) { //ds update position pLandmark->vecPointXYZOptimized = pVertex->estimate( )-p_vecTranslationToG2o; @@ -1194,3 +1407,84 @@ void Cg2oOptimizer::_applyOptimization( const UIDFrame& p_uFrame, const Eigen::V } } } + +void Cg2oOptimizer::_backPropagateTrajectoryToFull( const std::vector< CLoopClosureRaw >& p_vecClosures ) +{ + //ds back propagate the trajectory only graph into the full one + for( const CKeyFrame* pKeyFrame: m_vecKeyFramesInGraph ) + { + //ds search vertices + const g2o::HyperGraph::VertexIDMap::iterator itPose( m_cOptimizerSparse.vertices( ).find( pKeyFrame->uID+m_uIDShift ) ); + const g2o::HyperGraph::VertexIDMap::iterator itPoseTrajectoryOnly( m_cOptimizerSparseTrajectoryOnly.vertices( ).find( pKeyFrame->uID+1 ) ); + + //ds must exist + assert( m_cOptimizerSparse.vertices( ).end( ) != itPose ); + assert( m_cOptimizerSparseTrajectoryOnly.vertices( ).end( ) != itPoseTrajectoryOnly ); + + //ds //ds update estimate + g2o::VertexSE3* pVertexPose = dynamic_cast< g2o::VertexSE3* >( itPose->second ); + g2o::VertexSE3* pVertexPoseTrajectoryOnly = dynamic_cast< g2o::VertexSE3* >( itPoseTrajectoryOnly->second ); + pVertexPose->setEstimate( pVertexPoseTrajectoryOnly->estimate( ) ); + + //ds fetch pose egdes + const std::pair< g2o::EdgeSE3*, g2o::EdgeSE3* > prEdges( m_mapEdgeIDs[pKeyFrame->uID] ); + + //ds update connecting edge measurement + g2o::EdgeSE3* pEdge = dynamic_cast< g2o::EdgeSE3* >( *pVertexPose->edges( ).find( prEdges.first ) ); + g2o::EdgeSE3* pEdgeTrajectoryOnly = dynamic_cast< g2o::EdgeSE3* >( *pVertexPoseTrajectoryOnly->edges( ).find( prEdges.second ) ); + pEdge->setMeasurement( pEdgeTrajectoryOnly->measurement( ) ); + } + + //ds back propagate loop closures to full graph + for( CLoopClosureRaw cLoopClosure: p_vecClosures ) + { + //ds search vertices in full graph + const g2o::HyperGraph::VertexIDMap::iterator itPoseReference( m_cOptimizerSparse.vertices( ).find( cLoopClosure.uIDReference+m_uIDShift ) ); + const g2o::HyperGraph::VertexIDMap::iterator itPoseQuery( m_cOptimizerSparse.vertices( ).find( cLoopClosure.uIDQuery+m_uIDShift ) ); + + //ds must exist + assert( m_cOptimizerSparse.vertices( ).end( ) != itPoseReference ); + assert( m_cOptimizerSparse.vertices( ).end( ) != itPoseQuery ); + + //ds find edge in optimized graph + const g2o::HyperGraph::EdgeSet::iterator itEdgeLoopClosure( m_cOptimizerSparseTrajectoryOnly.edges( ).find( cLoopClosure.pEdgeLoopClosureReferenceToQuery ) ); + assert( m_cOptimizerSparseTrajectoryOnly.edges( ).end( ) != itEdgeLoopClosure ); + g2o::EdgeSE3* pEdgeLoopClosureInGraph = dynamic_cast< g2o::EdgeSE3* >( *itEdgeLoopClosure ); + + //ds set loop closure edge for full graph + g2o::EdgeSE3* pEdgeLoopClosure = new g2o::EdgeSE3( ); + pEdgeLoopClosure->setVertex( 0, dynamic_cast< g2o::VertexSE3* >( itPoseReference->second ) ); + pEdgeLoopClosure->setVertex( 1, dynamic_cast< g2o::VertexSE3* >( itPoseQuery->second ) ); + pEdgeLoopClosure->setMeasurement( pEdgeLoopClosureInGraph->measurement( ) ); + pEdgeLoopClosure->setInformation( m_matInformationLoopClosure ); + m_cOptimizerSparse.addEdge( pEdgeLoopClosure ); + } +} + +void Cg2oOptimizer::_backPropagateTrajectoryToPure( ) +{ + //ds back propagate the trajectory only graph into the full one + for( const CKeyFrame* pKeyFrame: m_vecKeyFramesInGraph ) + { + //ds search vertices + const g2o::HyperGraph::VertexIDMap::iterator itPose( m_cOptimizerSparse.vertices( ).find( pKeyFrame->uID+m_uIDShift ) ); + const g2o::HyperGraph::VertexIDMap::iterator itPoseTrajectoryOnly( m_cOptimizerSparseTrajectoryOnly.vertices( ).find( pKeyFrame->uID+1 ) ); + + //ds must exist + assert( m_cOptimizerSparse.vertices( ).end( ) != itPose ); + assert( m_cOptimizerSparseTrajectoryOnly.vertices( ).end( ) != itPoseTrajectoryOnly ); + + //ds //ds update estimate + g2o::VertexSE3* pVertexPose = dynamic_cast< g2o::VertexSE3* >( itPose->second ); + g2o::VertexSE3* pVertexPoseTrajectoryOnly = dynamic_cast< g2o::VertexSE3* >( itPoseTrajectoryOnly->second ); + pVertexPoseTrajectoryOnly->setEstimate( pVertexPose->estimate( ) ); + + //ds fetch pose egdes + const std::pair< g2o::EdgeSE3*, g2o::EdgeSE3* > prEdges( m_mapEdgeIDs[pKeyFrame->uID] ); + + //ds update connecting edge measurement + g2o::EdgeSE3* pEdge = dynamic_cast< g2o::EdgeSE3* >( *pVertexPose->edges( ).find( prEdges.first ) ); + g2o::EdgeSE3* pEdgeTrajectoryOnly = dynamic_cast< g2o::EdgeSE3* >( *pVertexPoseTrajectoryOnly->edges( ).find( prEdges.second ) ); + pEdgeTrajectoryOnly->setMeasurement( pEdge->measurement( ) ); + } +} diff --git a/src/optimization/Cg2oOptimizer.h b/src/optimization/Cg2oOptimizer.h index f0d3819..e01db90 100644 --- a/src/optimization/Cg2oOptimizer.h +++ b/src/optimization/Cg2oOptimizer.h @@ -21,6 +21,38 @@ class Cg2oOptimizer eOFFSET_IMUtoLEFT = 3 }; + struct CLoopClosureRaw + { + const std::vector< CKeyFrame* >::size_type uIDReference; + const std::vector< CKeyFrame* >::size_type uIDQuery; + g2o::EdgeSE3* pEdgeLoopClosureReferenceToQuery; + + CLoopClosureRaw( const std::vector< CKeyFrame* >::size_type& p_uIDReference, + const std::vector< CKeyFrame* >::size_type& p_uIDQuery, + g2o::EdgeSE3* p_pEdgeLoopClosureReferenceToQuery ): uIDReference( p_uIDReference ), + uIDQuery( p_uIDQuery ), + pEdgeLoopClosureReferenceToQuery( p_pEdgeLoopClosureReferenceToQuery ) + { + //ds nothing to do + } + }; + + struct CLoopClosure + { + const std::vector< CKeyFrame* >::size_type uIDReference; + const std::vector< CKeyFrame* >::size_type uIDQuery; + const Eigen::Isometry3d matTransformationReferenceToQuery; + + CLoopClosure( const std::vector< CKeyFrame* >::size_type& p_uIDReference, + const std::vector< CKeyFrame* >::size_type& p_uIDQuery, + const Eigen::Isometry3d& p_matTransformationReferenceToQuery ): uIDReference( p_uIDReference ), + uIDQuery( p_uIDQuery ), + matTransformationReferenceToQuery( p_matTransformationReferenceToQuery ) + { + //ds nothing to do + } + }; + public: Cg2oOptimizer( const std::shared_ptr< CStereoCamera > p_pCameraSTEREO, @@ -39,10 +71,12 @@ class Cg2oOptimizer const std::shared_ptr< std::vector< CKeyFrame* > > m_vecKeyFrames; g2o::SparseOptimizer m_cOptimizerSparse; - const UIDKeyFrame m_uIDShift = 1000000; //ds required to navigate between landmarks and poses - g2o::VertexSE3* m_pVertexPoseLAST = 0; + g2o::SparseOptimizer m_cOptimizerSparseTrajectoryOnly; + const int64_t m_uIDShift = 1000000; //ds required to navigate between landmarks and poses + //g2o::VertexSE3* m_pVertexPoseLAST = 0; const uint32_t m_uIterations = 100; //1000; uint32_t m_uOptimizations = 0; + std::vector< CKeyFrame* >::size_type m_uIDKeyFrameFrom = 0; const double m_dMaximumReliableDepthForPointXYZ = 2.5; const double m_dMaximumReliableDepthForUVDepth = 7.5; @@ -50,6 +84,7 @@ class Cg2oOptimizer const double m_dMaximumReliableDepthForPointXYZL2 = 10.0; const double m_dMaximumReliableDepthForUVDepthL2 = 50.0; const double m_dMaximumReliableDepthForUVDisparityL2 = 10000.0; + const double m_dSanePositionThresholdL2 = 1e12; //ds optimized structures std::vector< CLandmark* > m_vecLandmarksInGraph; @@ -61,6 +96,12 @@ class Cg2oOptimizer //ds stiffness for loop closing std::vector< g2o::EdgeSE3* > m_vecPoseEdges; + //ds hasmap for easy edge id access for connected key frames + std::map< std::vector< CKeyFrame* >::size_type, std::pair< g2o::EdgeSE3*, g2o::EdgeSE3* > > m_mapEdgeIDs; + + //ds total time spent in optimization + double m_dTotalOptimizationDurationSeconds = 0.0; + //ds information matrices ground structures private: @@ -73,7 +114,7 @@ class Cg2oOptimizer ClosureBuffer m_cBufferClosures; LoopClosureChecker m_cClosureChecker; - const double m_dMaximumThresholdLoopClosing = 500.0; + const double m_dMaximumThresholdLoopClosing = 0.5; public: @@ -98,6 +139,12 @@ class Cg2oOptimizer const std::vector< CKeyFrame* >::size_type& p_uIDEndKeyFrame, const Eigen::Vector3d& p_vecTranslationToG2o ); + const double getTotalOptimizationDurationSeconds( ) const { return m_dTotalOptimizationDurationSeconds; } + + /*ds manual trajectory locking + void lockTrajectory( const std::vector< CKeyFrame* >::size_type& p_uIDBeginKeyFrame, + const std::vector< CKeyFrame* >::size_type& p_uIDEndKeyFrame );*/ + /*ds first pose void updateSTART( const Eigen::Vector3d& p_vecTranslationWORLD ) { @@ -164,7 +211,7 @@ class Cg2oOptimizer g2o::EdgeSE3* _getEdgeLoopClosure( g2o::VertexSE3* p_pVertexPoseCurrent, const CKeyFrame* pKeyFrameCurrent, const CKeyFrame::CMatchICP* p_pClosure ); void _loadLandmarksToGraph( const std::vector< CLandmark* >::size_type& p_uIDLandmark, const Eigen::Vector3d& p_vecTranslationToG2o ); - g2o::VertexSE3* _setAndgetPose( g2o::VertexSE3* p_pVertexPoseFrom, CKeyFrame* pKeyFrameCurrent, const Eigen::Vector3d& p_vecTranslationToG2o ); + g2o::VertexSE3* _setAndgetPose( std::vector< CKeyFrame* >::size_type& p_uIDKeyFrameFrom, CKeyFrame* pKeyFrameCurrent, const Eigen::Vector3d& p_vecTranslationToG2o ); void _setLoopClosure( g2o::VertexSE3* p_pVertexPoseCurrent, const CKeyFrame* pKeyFrameCurrent, const CKeyFrame::CMatchICP* p_pClosure, const Eigen::Vector3d& p_vecTranslationToG2o ); void _setLandmarkMeasurementsWORLD( g2o::VertexSE3* p_pVertexPoseCurrent, const CKeyFrame* pKeyFrameCurrent, @@ -175,16 +222,30 @@ class Cg2oOptimizer void _applyOptimization( const UIDFrame& p_uFrame, const std::vector< CLandmark* >::size_type& p_uIDLandmark, const Eigen::Vector3d& p_vecTranslationToG2o ); void _applyOptimization( const UIDFrame& p_uFrame, const Eigen::Vector3d& p_vecTranslationToG2o ); - const Eigen::Matrix< double, 6, 6 > _getInformationWeakZ( const Eigen::Matrix< double, 6, 6 >& p_matInformationIN ) const + /*const Eigen::Matrix< double, 6, 6 > _getInformationWeakZ( const Eigen::Matrix< double, 6, 6 >& p_matInformationIN ) const { Eigen::Matrix< double, 6, 6 > matInformationOUT( p_matInformationIN ); //ds lower z by a factor matInformationOUT(5,5) /= 1e5; + return matInformationOUT; + }*/ + + const Eigen::Matrix< double, 6, 6 > _getInformationNoOrientation( ) const + { + Eigen::Matrix< double, 6, 6 > matInformationOUT( Eigen::Matrix< double, 6, 6 >::Identity( ) ); + + //ds cancel out orientation information + matInformationOUT(3,3) = 0.0; + matInformationOUT(4,4) = 0.0; + matInformationOUT(5,5) = 0.0; + return matInformationOUT; } + void _backPropagateTrajectoryToFull( const std::vector< CLoopClosureRaw >& p_vecClosures ); + void _backPropagateTrajectoryToPure( ); }; diff --git a/src/runnable/evaluate_trajectory.cpp b/src/runnable/evaluate_trajectory.cpp index 20601e8..6688c86 100644 --- a/src/runnable/evaluate_trajectory.cpp +++ b/src/runnable/evaluate_trajectory.cpp @@ -5,6 +5,8 @@ //ds custom #include "utility/CLogger.h" +#include "configuration/CConfigurationCameraKITTI.h" + //ds KITTI snippet @@ -20,7 +22,6 @@ int32_t main( int32_t argc, char **argv ) { std::printf( "(main) please specify the arguments \n" ); std::printf( "(main) terminated: %s\n", argv[0] ); - std::fflush( stdout ); return 1; } @@ -37,7 +38,6 @@ int32_t main( int32_t argc, char **argv ) { std::printf( "(main) unable to open trajectory files\n" ); std::printf( "(main) terminated: %s\n", argv[0] ); - std::fflush( stdout ); return 1; } @@ -45,20 +45,22 @@ int32_t main( int32_t argc, char **argv ) CLogger::openBox( ); std::printf( "(main) strInfileReference := '%s'\n", strInfileReference.c_str( ) ); std::printf( "(main) strInfileTest := '%s'\n", strInfileTest.c_str( ) ); - std::fflush( stdout ); CLogger::closeBox( ); //ds pose maps (for easy comparison) - std::map< const UIDFrame, const Eigen::Isometry3d > mapPosesReference; + std::map< const UIDFrame, Eigen::Isometry3d > mapPosesReference; std::map< const UIDFrame, const Eigen::Isometry3d > mapPosesTest; //ds frame counters UIDFrame uFrameCountReference = 0; - UIDFrame uFrameCountTest = 0; + std::vector< UIDFrame > vecFrameTest( 0 ); //ds parse both files try { + //ds to our WORLD + const Eigen::Isometry3d matTransformationLEFTtoWORLDInitial( Eigen::Matrix4d( CConfigurationCameraKITTI::matTransformationIntialLEFTtoWORLD ) ); + //ds reference first while( ifReference.good( ) ) { @@ -88,7 +90,8 @@ int32_t main( int32_t argc, char **argv ) const Eigen::Isometry3d matTransformationLEFTtoWORLD( matTransformationRAW ); //ds save to map - mapPosesReference.insert( std::make_pair( uFrameCountReference, matTransformationLEFTtoWORLD ) ); + //mapPosesReference.insert( std::make_pair( uFrameCountReference, matTransformationLEFTtoWORLDInitial*matTransformationLEFTtoWORLD ) ); + mapPosesReference.insert( std::make_pair( uFrameCountReference, matTransformationLEFTtoWORLD.inverse( ) ) ); ++uFrameCountReference; } } @@ -110,6 +113,10 @@ int32_t main( int32_t argc, char **argv ) //ds get it to a stringstream std::istringstream issLine( strLineBuffer ); + //ds parse frame id + UIDFrame uID = 0; + issLine >> uID; + //ds information fields (KITTI format) Eigen::Matrix4d matTransformationRAW( Eigen::Matrix4d::Identity( ) ); for( uint8_t u = 0; u < 3; ++u ) @@ -124,83 +131,155 @@ int32_t main( int32_t argc, char **argv ) const Eigen::Isometry3d matTransformationLEFTtoWORLD( matTransformationRAW ); //ds save to map - mapPosesTest.insert( std::make_pair( uFrameCountTest, matTransformationLEFTtoWORLD ) ); - ++uFrameCountTest; + mapPosesTest.insert( std::make_pair( uID, matTransformationLEFTtoWORLD.inverse( ) ) ); + vecFrameTest.push_back( uID ); } } + //ds synchronize initial values + mapPosesTest.insert( std::make_pair( 0, matTransformationLEFTtoWORLDInitial.inverse( )*mapPosesReference[0] ) ); + std::printf( "(main) successfully parsed: %s [entries: %lu]\n", strInfileTest.c_str( ), mapPosesTest.size( ) ); + + //std::cout << mapPosesReference[0].matrix( ) << std::endl; + //std::cout << mapPosesTest[0].matrix( ) << std::endl; + + /*ds buffer first vectors + const Eigen::Vector3d vecTranslationReference( mapPosesReference[vecFrameTest.front( )].translation( ).normalized( ) ); + const Eigen::Vector3d vecTranslationTest( mapPosesTest[vecFrameTest.front( )].translation( ).normalized( ) ); + + //ds compute rotation matrix + const Eigen::Vector3d vecCross( vecTranslationReference.cross( vecTranslationTest ) ); + const double dSine = vecCross.norm( ); + const double dCosine = vecTranslationReference.transpose( )*vecTranslationTest; + + //ds skew matrix + Eigen::Matrix3d matSkew; + + //ds formula: http://en.wikipedia.org/wiki/Cross_product#Skew-symmetric_matrix + matSkew << 0.0, -vecCross(2), vecCross(1), + vecCross(2), 0.0, -vecCross(0), + -vecCross(1), vecCross(0), 0.0; + + //ds get transformation matrix + Eigen::Isometry3d matTransformationReferenceToTest( Eigen::Matrix4d::Identity( ) ); + matTransformationReferenceToTest.linear( ) = Eigen::Matrix3d::Identity( ) + matSkew + ( 1-dCosine )/( dSine*dSine )*matSkew*matSkew; + + //ds adjust reference input to our coordinate frame + for( std::map< const UIDFrame, Eigen::Isometry3d >::size_type uID = 0; uID < mapPosesReference.size( ); ++uID ) + { + mapPosesReference[uID] = matTransformationReferenceToTest*mapPosesReference[uID]; + }*/ } catch( const std::exception& p_cException ) { //ds halt on any exception std::printf( "(main) ERROR: unable to parse trajectory files, exception: '%s'\n", p_cException.what( ) ); std::printf( "(main) terminated: %s\n", argv[0] ); - std::fflush( stdout ); return 1; } + //ds test frames + const UIDFrame uFrameCountTest = vecFrameTest.size( ); + //ds consistency if( uFrameCountTest > uFrameCountReference ) { std::printf( "(main) ERROR: test frame count (%lu) has to be lower than reference frame count (%lu)\n", uFrameCountTest, uFrameCountReference ); std::printf( "(main) terminated: %s\n", argv[0] ); - std::fflush( stdout ); return 1; } //ds stats double dTranslationErrorL1MetersTotal = 0.0; - double dTranslationTotalMeters = 0.0; + double dTranslationTotalMetersReference = 0.0; + double dTranslationTotalMetersTest = 0.0; double dRotationErrorTotal = 0.0; double dTranslationErrorL1RelativeTotal = 0.0; double dRotationErrorRelativeTotal = 0.0; - Eigen::Isometry3d matTransformationLEFTtoWORLDPrevious( Eigen::Matrix4d::Identity( ) ); - - //ds start framewise evaluation - for( UIDFrame uFrameCheck = 0; uFrameCheck < uFrameCountTest; ++uFrameCheck ) + try { - //ds fetch both transformations - const Eigen::Isometry3d matTransformationLEFTtoWORLDReference( mapPosesReference.at( uFrameCheck ) ); - const Eigen::Isometry3d matTransformationLEFTtoWORLDTest( mapPosesTest.at( uFrameCheck ) ); - - //ds translations - const double dTranslationL1Meters = ( matTransformationLEFTtoWORLDReference.translation( )-matTransformationLEFTtoWORLDPrevious.translation( ) ).norm( ); - const double dTranslationErrorL1Meters = ( matTransformationLEFTtoWORLDReference.translation( )-matTransformationLEFTtoWORLDTest.translation( ) ).norm( ); + //ds previous ID + UIDFrame uIDFramePrevious = 0; - //ds totals - if( 0.0 != dTranslationL1Meters ) + //ds start framewise evaluation + for( const UIDFrame uIDFrameTest: vecFrameTest ) { - dTranslationErrorL1MetersTotal += dTranslationErrorL1Meters; - dTranslationErrorL1RelativeTotal += dTranslationErrorL1Meters/dTranslationL1Meters; - } + //ds compute relative transformations + const Eigen::Isometry3d matTransformationRelativeReference( mapPosesReference[uIDFrameTest]*mapPosesReference[uIDFramePrevious].inverse( ) ); + const Eigen::Isometry3d matTransformationRelativeTest( mapPosesTest[uIDFrameTest]*mapPosesTest[uIDFramePrevious].inverse( ) ); - //ds rotation error - const double dRotation = rotationError( matTransformationLEFTtoWORLDReference.linear( )-matTransformationLEFTtoWORLDPrevious.linear( ) ); - const double dRotationError = rotationError( matTransformationLEFTtoWORLDReference.linear( )-matTransformationLEFTtoWORLDTest.linear( ) ); + //ds ignore z value + //matTransformationLEFTtoWORLDReference.translation( ).z( ) = 0.0; + //matTransformationLEFTtoWORLDTest.translation( ).z( ) = 0.0; - //ds totals - if( 0.0 != dRotation ) - { - dRotationErrorTotal += dRotationError; - dRotationErrorRelativeTotal += dRotationError/dRotation; - } + //ds translations + const double dTranslationL1Meters = matTransformationRelativeReference.translation( ).norm( ); + const double dTranslationErrorL1Meters = ( matTransformationRelativeReference.translation( )-matTransformationRelativeTest.translation( ) ).norm( ); - //ds accumulate total translation - dTranslationTotalMeters += dTranslationL1Meters; - matTransformationLEFTtoWORLDPrevious = matTransformationLEFTtoWORLDReference; + //ds mark big errors + if( 1.0 < dTranslationErrorL1Meters/dTranslationL1Meters ) + { + std::printf( "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX\n" ); + } + + std::printf( "(main) [%06lu][%06lu] error: %5.2f/%5.2f (%7.2f, %7.2f, %7.2f) > (%7.2f, %7.2f, %7.2f)\n", + uIDFramePrevious, uIDFrameTest, dTranslationErrorL1Meters, dTranslationL1Meters, + matTransformationRelativeReference.translation( ).x( ), matTransformationRelativeReference.translation( ).y( ), matTransformationRelativeReference.translation( ).z( ), + matTransformationRelativeTest.translation( ).x( ), matTransformationRelativeTest.translation( ).y( ), matTransformationRelativeTest.translation( ).z( ) ); + + //ds mark big errors + if( 1.0 < dTranslationErrorL1Meters/dTranslationL1Meters ) + { + std::printf( "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX\n" ); + } + + //ds totals + if( 0.0 != dTranslationL1Meters ) + { + dTranslationErrorL1MetersTotal += dTranslationErrorL1Meters; + dTranslationErrorL1RelativeTotal += dTranslationErrorL1Meters/dTranslationL1Meters; + } + + //ds rotation error + const double dRotation = rotationError( matTransformationRelativeReference.linear( ) ); + const double dRotationError = rotationError( matTransformationRelativeReference.linear( )-matTransformationRelativeTest.linear( ) ); + + //ds totals + if( 0.0 != dRotation ) + { + dRotationErrorTotal += dRotationError; + dRotationErrorRelativeTotal += dRotationError/dRotation; + } + + //ds accumulate total translation + dTranslationTotalMetersReference += dTranslationL1Meters; + dTranslationTotalMetersTest += matTransformationRelativeTest.translation( ).norm( ); + + //ds update previous + uIDFramePrevious = uIDFrameTest; + } + } + catch( const std::exception& p_cException ) + { + //ds halt on any exception + std::printf( "(main) ERROR: unable to parse imported trajectory vectors, exception: '%s'\n", p_cException.what( ) ); + std::printf( "(main) terminated: %s\n", argv[0] ); + return 1; } CLogger::openBox( ); - std::printf( "(main) total trajectory length = %f\n", dTranslationTotalMeters ); - std::printf( "(main) translation error (L1) total = %f\n", dTranslationErrorL1MetersTotal ); - std::printf( "(main) translation error (L1) average = %f\n", dTranslationErrorL1MetersTotal/uFrameCountTest ); - std::printf( "(main) translation error (L1) relative = %f\n", dTranslationErrorL1RelativeTotal/uFrameCountTest ); - std::printf( "(main) rotation error total = %f\n", dRotationErrorTotal ); - std::printf( "(main) rotation error average = %f\n", dRotationErrorTotal/uFrameCountTest ); + std::printf( "(main) RESULT SUMMARY:\n\n" ); + std::printf( "(main) total trajectory length = %f m\n", dTranslationTotalMetersReference ); + std::printf( "(main) computed trajectory length = %f m\n\n", dTranslationTotalMetersTest ); + std::printf( "(main) translation error (L1) total = %f m\n", dTranslationErrorL1MetersTotal ); + std::printf( "(main) translation error (L1) average = %f m\n", dTranslationErrorL1MetersTotal/uFrameCountTest ); + std::printf( "(main) translation error (L1) relative = %f \n", dTranslationErrorL1RelativeTotal/uFrameCountTest ); + std::printf( "(main) relative translation precision = [%f]\n\n", 1.0-dTranslationErrorL1RelativeTotal/uFrameCountTest ); + std::printf( "(main) rotation error total = %f rad\n", dRotationErrorTotal ); + std::printf( "(main) rotation error average = %f rad\n", dRotationErrorTotal/uFrameCountTest ); std::printf( "(main) rotation error relative = %f\n", dRotationErrorRelativeTotal/uFrameCountTest ); - std::fflush( stdout ); CLogger::closeBox( ); //ds done @@ -209,7 +288,6 @@ int32_t main( int32_t argc, char **argv ) //ds exit std::printf( "(main) terminated: %s\n", argv[0] ); - std::fflush( stdout); return 0; } diff --git a/src/runnable/republisher_kitti.cpp b/src/runnable/republisher_kitti.cpp index 4244ae2..9a3ab1f 100644 --- a/src/runnable/republisher_kitti.cpp +++ b/src/runnable/republisher_kitti.cpp @@ -87,9 +87,9 @@ int32_t main( int32_t argc, char **argv ) }*/ //ds default files - std::string strInfileTimestamps = "/media/n551jw/data/n551jw/Downloads/dataset/sequences/15/times.txt"; - std::string strImageFolderLEFT = "/media/n551jw/data/n551jw/Downloads/dataset/sequences/15/image_0/"; - std::string strImageFolderRIGHT = "/media/n551jw/data/n551jw/Downloads/dataset/sequences/15/image_1/"; + std::string strInfileTimestamps = "/media/n551jw/data/n551jw/Downloads/dataset/sequences/21/times.txt"; + std::string strImageFolderLEFT = "/media/n551jw/data/n551jw/Downloads/dataset/sequences/21/image_0/"; + std::string strImageFolderRIGHT = "/media/n551jw/data/n551jw/Downloads/dataset/sequences/21/image_1/"; std::string strOutfile = argv[1]; //ds open outfile diff --git a/src/runnable/stereo_fps.cpp b/src/runnable/stereo_fps.cpp index f86f725..7745765 100644 --- a/src/runnable/stereo_fps.cpp +++ b/src/runnable/stereo_fps.cpp @@ -245,9 +245,11 @@ int32_t main( int32_t argc, char **argv ) } //ds get end time - const double dDuration = CLogger::getTimeSeconds( )-dTimeStartSeconds; + const double dDurationTotal = CLogger::getTimeSeconds( )-dTimeStartSeconds; + const double dDurationG2o = cTracker.getTotalDurationOptimizationSeconds( ); + const double dDurationPure = dDurationTotal-dDurationG2o; const UIDFrame uFrameCount = cTracker.getFrameCount( ); - const double dDurationOnline = uFrameCount/20.0; + const double dDurationOnline = uFrameCount/10.0; const double dDistance = cTracker.getDistanceTraveled( ); if( 1 < uFrameCount ) @@ -266,10 +268,11 @@ int32_t main( int32_t argc, char **argv ) CLogger::openBox( ); std::printf( "(main) dataset completed\n" ); std::printf( "(main) distance traveled: %fm\n", dDistance ); - std::printf( "(main) duration: %fs (online: %fs, x%f)\n", dDuration, dDurationOnline, dDuration/dDurationOnline ); + std::printf( "(main) duration: %fs (online: %fs, x%f)\n", dDurationTotal, dDurationOnline, dDurationTotal/dDurationOnline ); + std::printf( "(main) duration pure: %fs (g2o: %fs)\n", dDurationPure, dDurationG2o ); std::printf( "(main) traveling speed (online): %fm/s\n", dDistance/dDurationOnline ); std::printf( "(main) total frames: %lu\n", uFrameCount ); - std::printf( "(main) frame rate (avg): %f FPS\n", uFrameCount/dDuration ); + std::printf( "(main) frame rate (avg): %f FPS\n", uFrameCount/dDurationPure ); std::printf( "(main) invalid frames: %li (%4.2f)\n", uInvalidFrames, static_cast< double >( uInvalidFrames )/uFrameCount ); CLogger::closeBox( ); diff --git a/src/runnable/stereo_fps_kitti.cpp b/src/runnable/stereo_fps_kitti.cpp index d040815..7778d70 100644 --- a/src/runnable/stereo_fps_kitti.cpp +++ b/src/runnable/stereo_fps_kitti.cpp @@ -88,8 +88,9 @@ int32_t main( int32_t argc, char **argv ) CLogger::CLogLandmarkCreation::open( ); CLogger::CLogLandmarkFinal::open( ); CLogger::CLogLandmarkFinalOptimized::open( ); - CLogger::CLogOptimizationOdometry::open( );*/ - CLogger::CLogTrajectory::open( ); + CLogger::CLogOptimizationOdometry::open( ); + CLogger::CLogTrajectory::open( );*/ + CLogger::CLogTrajectoryKITTI::open( ); //ds log configuration std::printf( "(main) strMode := '%s'\n", strMode.c_str( ) ); @@ -197,9 +198,11 @@ int32_t main( int32_t argc, char **argv ) } //ds get end time - const double dDuration = CLogger::getTimeSeconds( )-dTimeStartSeconds; + const double dDurationTotal = CLogger::getTimeSeconds( )-dTimeStartSeconds; + const double dDurationG2o = cTracker.getTotalDurationOptimizationSeconds( ); + const double dDurationPure = dDurationTotal-dDurationG2o; const UIDFrame uFrameCount = cTracker.getFrameCount( ); - const double dDurationOnline = uFrameCount/20.0; + const double dDurationOnline = uFrameCount/10.0; const double dDistance = cTracker.getDistanceTraveled( ); if( 1 < uFrameCount ) @@ -218,10 +221,11 @@ int32_t main( int32_t argc, char **argv ) CLogger::openBox( ); std::printf( "(main) dataset completed\n" ); std::printf( "(main) distance traveled: %fm\n", dDistance ); - std::printf( "(main) duration: %fs (online: %fs, x%f)\n", dDuration, dDurationOnline, dDuration/dDurationOnline ); + std::printf( "(main) duration: %fs (online: %fs, x%f)\n", dDurationTotal, dDurationOnline, dDurationTotal/dDurationOnline ); + std::printf( "(main) duration pure: %fs (g2o: %fs)\n", dDurationPure, dDurationG2o ); std::printf( "(main) traveling speed (online): %fm/s\n", dDistance/dDurationOnline ); std::printf( "(main) total frames: %lu\n", uFrameCount ); - std::printf( "(main) frame rate (avg): %f FPS\n", uFrameCount/dDuration ); + std::printf( "(main) frame rate (avg): %f FPS\n", uFrameCount/dDurationPure ); std::printf( "(main) invalid frames: %li (%4.2f)\n", uInvalidFrames, static_cast< double >( uInvalidFrames )/uFrameCount ); CLogger::closeBox( ); diff --git a/src/utility/CLogger.cpp b/src/utility/CLogger.cpp index e0a9429..cc0af1f 100644 --- a/src/utility/CLogger.cpp +++ b/src/utility/CLogger.cpp @@ -9,3 +9,4 @@ std::FILE* CLogger::CLogLandmarkFinalOptimized::m_pFile = 0; std::FILE* CLogger::CLogDetectionEpipolar::m_pFile = 0; std::FILE* CLogger::CLogOptimizationOdometry::m_pFile = 0; std::FILE* CLogger::CLogIMUInput::m_pFile = 0; +std::ofstream CLogger::CLogTrajectoryKITTI::ofOutfile; diff --git a/src/utility/CLogger.h b/src/utility/CLogger.h index 877d4c3..0f03b38 100644 --- a/src/utility/CLogger.h +++ b/src/utility/CLogger.h @@ -289,7 +289,7 @@ class CLogger assert( 0 != m_pFile ); std::fprintf( m_pFile, "ID_FRAME | LINEAR ACCELERATION: X Y Z | FILTERED: X Y Z | ANGULAR VELOCITY: X Y Z | FILTERED: X Y Z\n" ); } - static void addEntry( const UIDFrame& p_uFrame, const CLinearAccelerationWORLD& p_vecLinearAcceleration, const CLinearAccelerationWORLD& p_vecLinearAccelerationFiltered, const CAngularVelocityLEFT& p_vecAngularVelocity, const CAngularVelocityLEFT& p_vecAngularVelocityFiltered ) + static void addEntry( const UIDFrame& p_uFrame, const CLinearAccelerationWORLD& p_vecLinearAcceleration, const CLinearAccelerationWORLD& p_vecLinearAccelerationFiltered, const CAngularVelocityLEFT& p_vecAngularVelocity, const CAngularVelocityLEFT& p_vecAngularVelocityFiltered ) { assert( 0 != m_pFile ); std::fprintf( m_pFile, " %04lu | %6.2f %6.2f %6.2f | %6.2f %6.2f %6.2f | %6.2f %6.2f %6.2f | %6.2f %6.2f %6.2f\n", p_uFrame, @@ -300,6 +300,46 @@ class CLogger } CLogIMUInput; + static struct CLogTrajectoryKITTI + { + static std::ofstream ofOutfile; + + static void open( ) + { + ofOutfile.open( "logs/trajectory_final_KITTI.txt", std::ofstream::out ); + assert( ofOutfile.good( ) ); + } + static void addEntry( const UIDFrame& p_uFrame, const Eigen::Isometry3d& p_matTransformationLEFTtoWORLD, const Eigen::Vector3d& p_vecTranslationToG2o ) + { + //ds clear translation to original frame + Eigen::Isometry3d matTransformationLEFTtoWORLDShifted( p_matTransformationLEFTtoWORLD ); + matTransformationLEFTtoWORLDShifted.translation( ) += p_vecTranslationToG2o; + + //ds get to matrix representation + const Eigen::Matrix4d matTransformationLEFTtoWORLD( matTransformationLEFTtoWORLDShifted.matrix( ) ); + + //ds KITTI 3x4 format + assert( ofOutfile.good( ) ); + + //ds frame number + ofOutfile << p_uFrame; + + //ds transformation matrix + for( uint8_t u = 0; u < 3; ++u ) + { + for( uint8_t v = 0; v < 4; ++v ) + { + ofOutfile << " " << matTransformationLEFTtoWORLD(u,v); + } + } + + //ds done + ofOutfile << "\n"; + } + static void close( ){ if( ofOutfile.good( ) ){ ofOutfile.close( ); } } + + } CLogTrajectoryKITTI; + }; #endif //CLOGGER_H