Skip to content

Commit

Permalink
updated
Browse files Browse the repository at this point in the history
  • Loading branch information
n551jw committed Nov 9, 2015
1 parent e86f915 commit 519e4b0
Show file tree
Hide file tree
Showing 13 changed files with 659 additions and 166 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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})
Expand Down
2 changes: 1 addition & 1 deletion src/core/CTrackerStereoMotionModel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 );
Expand Down
1 change: 1 addition & 0 deletions src/core/CTrackerStereoMotionModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
82 changes: 46 additions & 36 deletions src/core/CTrackerStereoMotionModelKITTI.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,9 @@ CTrackerStereoMotionModelKITTI::CTrackerStereoMotionModelKITTI( const EPlaybackM
std::printf( "[%06lu]<CTrackerStereoMotionModelKITTI>(CTrackerStereoMotionModelKITTI) minimum matches for loop closure: %lu\n", m_uFrameCount, m_uMinimumNumberOfMatchesLoopClosure );
std::printf( "[%06lu]<CTrackerStereoMotionModelKITTI>(CTrackerStereoMotionModelKITTI) instance allocated\n", m_uFrameCount );
CLogger::closeBox( );

//ds sanity check
assert( m_uMinimumLandmarksForKeyFrame >= m_uMinimumNumberOfMatchesLoopClosure );
}

CTrackerStereoMotionModelKITTI::~CTrackerStereoMotionModelKITTI( )
Expand All @@ -101,16 +104,17 @@ 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;
}

/*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>(~CTrackerStereoMotionModelKITTI) instance deallocated\n", m_uFrameCount );
}
Expand All @@ -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( );
Expand Down Expand Up @@ -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 );
Expand All @@ -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]<CTrackerStereoMotionModelKITTI>(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]<CTrackerStereoMotionModelKITTI>(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]<CTrackerStereoMotionModelKITTI>(finalize) found closures: %lu\n", m_uFrameCount, vecLoopClosures.size( ) );
std::printf( "[%06lu]<CTrackerStereoMotionModelKITTI>(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]<CTrackerStereoMotionModelKITTI>(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]<CTrackerStereoMotionModelKITTI>(finalize) closed keyframe: %06lu\n", m_uFrameCount, pKeyFrameForClosure->uID );
}
}
}

Expand All @@ -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]<CTrackerStereoMotionModelKITTI>(finalize) updating already optimized keyframes", m_uFrameCount );
std::printf( "[%06lu]<CTrackerStereoMotionModelKITTI>(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 );
}
}

Expand Down
9 changes: 5 additions & 4 deletions src/core/CTrackerStereoMotionModelKITTI.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;

Expand Down Expand Up @@ -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:
Expand Down
Loading

0 comments on commit 519e4b0

Please sign in to comment.