Skip to content

Commit

Permalink
updated
Browse files Browse the repository at this point in the history
  • Loading branch information
n551jw committed Sep 27, 2015
1 parent c7d05c3 commit 6f2d88b
Show file tree
Hide file tree
Showing 9 changed files with 927 additions and 105 deletions.
506 changes: 504 additions & 2 deletions src/core/CFundamentalMatcher.cpp
View file

Large diffs are not rendered by default.

15 changes: 13 additions & 2 deletions src/core/CFundamentalMatcher.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,8 +125,8 @@ class CFundamentalMatcher
const uint8_t m_uCapIterationsPoseOptimization = 100;
const double m_dConvergenceDeltaPoseOptimization = 1e-5;
const double m_dMaximumErrorInlierSquaredPixelsPoseOptimization = 10.0;
const double m_dMaximumErrorSquaredAveragePoseOptimization = 9.5;
const double m_dMaximumRISK = 0.5;
const double m_dMaximumErrorSquaredAveragePoseOptimization = 9.75;
const double m_dMaximumRISK = 1.5;

//ds if the optimized pose has an combined squared translational change less than this value it gets ignored
const double m_dTranslationResolutionOptimization = 0.001;
Expand Down Expand Up @@ -161,6 +161,17 @@ class CFundamentalMatcher
const Eigen::Vector3d& p_vecTranslationTotal,
const double& p_dMotionScaling );

const Eigen::Isometry3d getPoseOptimizedSTEREOUVfromLAST( const UIDFrame p_uFrame,
cv::Mat& p_matDisplayLEFT,
cv::Mat& p_matDisplayRIGHT,
const cv::Mat& p_matImageLEFT,
const cv::Mat& p_matImageRIGHT,
const Eigen::Isometry3d& p_matTransformationEstimateWORLDtoLEFT,
const Eigen::Isometry3d& p_matTransformationWORLDtoLEFTLAST,
const Eigen::Vector3d& p_vecRotationTotal,
const Eigen::Vector3d& p_vecTranslationTotal,
const double& p_dMotionScaling );

const Eigen::Isometry3d getPoseRefinedOnVisibleLandmarks( const Eigen::Isometry3d& p_matTransformationWORLDtoLEFTEstimate );

const std::shared_ptr< std::vector< const CMeasurementLandmark* > > getMeasurementsDummy( cv::Mat& p_matDisplayLEFT,
Expand Down
225 changes: 211 additions & 14 deletions src/core/CTrackerStereoMotionModel.cpp
View file

Large diffs are not rendered by default.

11 changes: 8 additions & 3 deletions src/core/CTrackerStereoMotionModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,8 @@ class CTrackerStereoMotionModel
const uint8_t m_uVisibleLandmarksMinimum;
const double m_dMinimumDepthMeters = 0.05;
const double m_dMaximumDepthMeters = 100.0;
const UIDFrame m_uMaximumNumberOfFramesWithoutDetection = 20;
UIDFrame m_uNumberOfFramesWithoutDetection = 0;

std::shared_ptr< CTriangulator > m_pTriangulator;
CFundamentalMatcher m_cMatcher;
Expand All @@ -82,12 +84,12 @@ class CTrackerStereoMotionModel
Cg2oOptimizer m_cGraphOptimizer;

//ds loop closing
const UIDKeyFrame m_uMinimumLoopClosingKeyFrameDistance = 10;
const UIDKeyFrame m_uMinimumLoopClosingKeyFrameDistance = 20;
const UIDLandmark m_uMinimumNumberOfMatchesLoopClosure = 25;
const UIDKeyFrame m_uLoopClosingKeyFrameWaitingQueue = 1;
UIDKeyFrame m_uLoopClosingKeyFramesInQueue = 0;
UIDKeyFrame m_uIDLoopClosureOptimizedLAST = 0;
const double m_dLoopClosingRadiusSquaredMeters = 100.0;
const double m_dLoopClosingRadiusSquaredMeters = 1000.0;

//ds control
const EPlaybackMode m_eMode;
Expand All @@ -100,6 +102,7 @@ class CTrackerStereoMotionModel
uint32_t m_uFramesCurrentCycle = 0;
double m_dPreviousFrameRate = 0.0;
double m_dPreviousFrameTime = 0.0;
double m_dDistanceTraveledMeters = 0.0;

//ds accessors
public:
Expand All @@ -117,6 +120,7 @@ class CTrackerStereoMotionModel
const std::pair< bool, Eigen::Isometry3d > getFrameLEFTtoWORLD( ){ m_bIsFrameAvailable = false; return m_prFrameLEFTtoWORLD; }
void finalize( );
void sanitizeFiletree( ){ m_cGraphOptimizer.clearFiles( ); }
const double getDistanceTraveled( ) const { return m_dDistanceTraveledMeters; }

//ds helpers
private:
Expand All @@ -138,7 +142,8 @@ class CTrackerStereoMotionModel
cv::Mat& p_matDisplaySTEREO );

//ds loop closing
const std::vector< const CKeyFrame::CMatchICP* > _getLoopClosuresKeyFrameFCFS( const UIDKeyFrame& p_uID, const Eigen::Isometry3d& p_matTransformationLEFTtoWORLD, const std::shared_ptr< const std::vector< CDescriptorVectorPoint3DWORLD > > p_vecCloudQuery );
const std::vector< const CKeyFrame::CMatchICP* > _getLoopClosuresForKeyFrame( const UIDKeyFrame& p_uID, const Eigen::Isometry3d& p_matTransformationLEFTtoWORLD, const std::shared_ptr< const std::vector< CDescriptorVectorPoint3DWORLD > > p_vecCloudQuery );
const std::vector< const CKeyFrame::CMatchICP* > _getLoopClosuresForKeyFrameDeep( const Eigen::Isometry3d& p_matTransformationLEFTtoWORLD, const std::shared_ptr< const std::vector< CDescriptorVectorPoint3DWORLD > > p_vecCloudQuery );

//ds control
void _shutDown( );
Expand Down
189 changes: 136 additions & 53 deletions src/optimization/Cg2oOptimizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,17 @@ Cg2oOptimizer::Cg2oOptimizer( const std::shared_ptr< CStereoCamera > p_pCameraST
m_pVertexPoseLAST->setEstimate( p_matTransformationLEFTtoWORLDInitial );
m_pVertexPoseLAST->setId( m_uIDShift-1 );
m_pVertexPoseLAST->setFixed( true );
m_cOptimizerSparse.addVertex( m_pVertexPoseLAST );

//ds always save acceleration data
//ds set to graph
m_cOptimizerSparse.addVertex( m_pVertexPoseLAST );
m_cOptimizerSparse.addEdge( _getEdgeLinearAcceleration( m_pVertexPoseLAST, Eigen::Vector3d( 0.0, -1.0, 0.0 ) ) );

//ds get a copy
m_pVertexPoseFIRST = new g2o::VertexSE3( );
m_pVertexPoseFIRST->setEstimate( p_matTransformationLEFTtoWORLDInitial );
m_pVertexPoseFIRST->setId( m_uIDShift-1 );
m_pVertexPoseFIRST->setFixed( true );

//std::printf( "<Cg2oOptimizer>(Cg2oOptimizer) configuration: gn_var_cholmod\n" );
std::printf( "<Cg2oOptimizer>(Cg2oOptimizer) instance allocated\n" );
CLogger::closeBox( );
Expand All @@ -44,11 +50,10 @@ 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( 100*Eigen::Matrix< double, 3, 3 >::Identity( ) )
{
m_vecLandmarksInGraph.clear( );
m_vecKeyFramesInGraph.clear( );
m_vecActivePosesInGraph.clear( );

//ds configure the solver (var_cholmod)
//m_cOptimizerSparse.setVerbose( true );
Expand Down Expand Up @@ -277,7 +282,7 @@ void Cg2oOptimizer::optimizeContinuous( const UIDKeyFrame& p_uIDBeginKeyFrame )
for( const CKeyFrame::CMatchICP* pClosure: pKeyFrame->vecLoopClosures )
{
//ds set it to the graph
_setLoopClosure( pVertexPoseCurrent, pKeyFrame, pClosure );
//_setLoopClosure( pVertexPoseCurrent, pKeyFrame, pClosure );
++uLoopClosures;
}

Expand All @@ -301,29 +306,49 @@ void Cg2oOptimizer::optimizeContinuous( const UIDKeyFrame& p_uIDBeginKeyFrame )
m_cOptimizerSparse.save( ( strPrefix + ".g2o" ).c_str( ) );

std::printf( "<Cg2oOptimizer>(optimizeContinuous) optimizing [keyframes: %06lu-%06lu (%3lu) landmarks: %06lu-%06lu][measurements xyz: %3lu depth: %3lu disparity: %3lu, loop closures: %3u] ",
vecChunkKeyFrames.front( )->uID, vecChunkKeyFrames.back( )->uID, m_vecActivePosesInGraph.size( ), vecChunkLandmarks.front( )->uID, vecChunkLandmarks.back( )->uID, uMeasurementsStoredPointXYZ, uMeasurementsStoredUVDepth, uMeasurementsStoredUVDisparity, uLoopClosures );
vecChunkKeyFrames.front( )->uID, vecChunkKeyFrames.back( )->uID, m_vecKeyFramesInGraph.size( ), vecChunkLandmarks.front( )->uID, vecChunkLandmarks.back( )->uID, uMeasurementsStoredPointXYZ, uMeasurementsStoredUVDepth, uMeasurementsStoredUVDisparity, uLoopClosures );

//ds initialize optimization
m_cOptimizerSparse.initializeOptimization( );

//ds do ten iterations
//ds do iterations
m_cOptimizerSparse.optimize( m_uIterations );

//ds save optimized situation
m_cOptimizerSparse.save( ( strPrefix + "_optimized.g2o" ).c_str( ) );

//ds update points
_applyOptimization( m_vecLandmarksInGraph );
_applyOptimization( m_vecKeyFramesInGraph );

//ds if there were loop closures
if( 0 < uLoopClosures )
{
//ds fix the trajectory
_fixateTrajectory( );
//ds save optimized situation
m_cOptimizerSparse.save( ( strPrefix + "_closed.g2o" ).c_str( ) );

//ds clear the graph as everything will be fixed for now
m_cOptimizerSparse.clear( );
m_vecLandmarksInGraph.clear( );
m_vecKeyFramesInGraph.clear( );

//ds buffer last keyframe
const CKeyFrame* pKeyFrameLAST = vecChunkKeyFrames.back( );

//ds add the copy of the last vertex again and lock it
m_pVertexPoseLAST = new g2o::VertexSE3( );
m_pVertexPoseLAST->setEstimate( pKeyFrameLAST->matTransformationLEFTtoWORLD );
m_pVertexPoseLAST->setId( pKeyFrameLAST->uID+m_uIDShift );
m_pVertexPoseLAST->setFixed( true );
m_cOptimizerSparse.addVertex( m_pVertexPoseLAST );

//ds include acceleration data
m_cOptimizerSparse.addEdge( _getEdgeLinearAcceleration( m_pVertexPoseLAST, pKeyFrameLAST->vecLinearAccelerationNormalized ) );
std::printf( "- cleared graph\n" );
}
else
{
//ds save optimized situation
m_cOptimizerSparse.save( ( strPrefix + "_optimized.g2o" ).c_str( ) );
std::printf( "- completed\n" );
}

std::printf( "- completed\n" );

//ds done
//std::printf( "<Cg2oOptimizer>(optimizeContinuous) optimized poses: %lu and landmarks: %lu (error final: %f)\n", vecChunkKeyFrames.size( ), vecChunkLandmarks.size( ), m_cOptimizerSparse.activeChi2( ) );
Expand Down Expand Up @@ -365,6 +390,58 @@ void Cg2oOptimizer::clearFiles( ) const
}
}

void Cg2oOptimizer::saveFinalGraph( )
{
//ds clear all structures
m_cOptimizerSparse.clear( );

//ds add the landmarks
m_vecLandmarksInGraph.clear( );
_loadLandmarksToGraph( *m_vecLandmarks );

//ds add first pose
m_pVertexPoseLAST = m_pVertexPoseFIRST;
m_cOptimizerSparse.addVertex( m_pVertexPoseLAST );
m_cOptimizerSparse.addEdge( _getEdgeLinearAcceleration( m_pVertexPoseLAST, Eigen::Vector3d( 0.0, -1.0, 0.0 ) ) );

//ds info
UIDLandmark uMeasurementsStoredPointXYZ = 0;
UIDLandmark uMeasurementsStoredUVDepth = 0;
UIDLandmark uMeasurementsStoredUVDisparity = 0;
uint32_t uLoopClosures = 0;

//ds loop over the camera vertices vector
for( const CKeyFrame* pKeyFrame: *m_vecKeyFrames )
{
//ds add current camera pose
g2o::VertexSE3* pVertexPoseCurrent = _setAndgetPose( m_pVertexPoseLAST, pKeyFrame );

//ds check if we got loop closures for this frame
for( const CKeyFrame::CMatchICP* pClosure: pKeyFrame->vecLoopClosures )
{
//ds set it to the graph
//_setLoopClosure( pVertexPoseCurrent, pKeyFrame, pClosure );
++uLoopClosures;
}

//ds always save acceleration data
m_cOptimizerSparse.addEdge( _getEdgeLinearAcceleration( pVertexPoseCurrent, pKeyFrame->vecLinearAccelerationNormalized ) );

//ds set landmark measurements
_setLandmarkMeasurementsWORLD( pVertexPoseCurrent, pKeyFrame, uMeasurementsStoredPointXYZ, uMeasurementsStoredUVDepth, uMeasurementsStoredUVDisparity );

//ds update last
m_pVertexPoseLAST = pVertexPoseCurrent;
}

//ds save to a file
m_cOptimizerSparse.save( "g2o/local/FINAL.g2o" );

std::printf( "<Cg2oOptimizer>(saveFinalGraph) saved [keyframes: %lu landmarks %lu][measurements xyz: %3lu depth: %3lu disparity: %3lu, loop closures: %3u]\n",
m_vecKeyFrames->size( ), m_vecLandmarksInGraph.size( ), uMeasurementsStoredPointXYZ, uMeasurementsStoredUVDepth, uMeasurementsStoredUVDisparity, uLoopClosures );

}

g2o::EdgeSE3LinearAcceleration* Cg2oOptimizer::_getEdgeLinearAcceleration( g2o::VertexSE3* p_pVertexPose,
const CLinearAccelerationIMU& p_vecLinearAccelerationNormalized ) const
{
Expand Down Expand Up @@ -523,7 +600,6 @@ g2o::VertexSE3* Cg2oOptimizer::_setAndgetPose( g2o::VertexSE3* p_pVertexPoseFrom
pVertexPoseCurrent->setEstimate( pKeyFrameCurrent->matTransformationLEFTtoWORLD );
pVertexPoseCurrent->setId( pKeyFrameCurrent->uID+m_uIDShift );
m_cOptimizerSparse.addVertex( pVertexPoseCurrent );
m_vecActivePosesInGraph.push_back( pVertexPoseCurrent );

//ds set up the edge to connect the poses
g2o::EdgeSE3* pEdgePoseFromTo = new g2o::EdgeSE3( );
Expand All @@ -545,30 +621,26 @@ void Cg2oOptimizer::_setLoopClosure( g2o::VertexSE3* p_pVertexPoseCurrent, const
//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
assert( itClosure != m_cOptimizerSparse.vertices( ).end( ) );

//ds pose to close
g2o::VertexSE3* pVertexClosure = dynamic_cast< g2o::VertexSE3* >( itClosure->second );

//ds fix reference
pVertexClosure->setFixed( true );
g2o::VertexSE3* pVertexClosure = 0;

/*ds if found
//ds if found
if( itClosure != m_cOptimizerSparse.vertices( ).end( ) )
{
//ds extract the pose
pVertexClosure = dynamic_cast< g2o::VertexSE3* >( itClosure->second );
}
else
{
//ds we have to add the pose and fix it
//ds we have to add the pose temporarily and fix it
pVertexClosure = new g2o::VertexSE3( );
pVertexClosure->setEstimate( p_pClosure->pKeyFrameReference->matTransformationLEFTtoWORLD );
pVertexClosure->setId( p_pClosure->pKeyFrameReference->uID+m_uIDShift );
pVertexClosure->setFixed( true );
m_cOptimizerSparse.addVertex( pVertexClosure );
}*/
}

//ds fix reference
pVertexClosure->setFixed( true );

//ds set up the edge
g2o::EdgeSE3* pEdgeLoopClosure( new g2o::EdgeSE3( ) );
Expand All @@ -592,22 +664,42 @@ void Cg2oOptimizer::_setLoopClosure( g2o::VertexSE3* p_pVertexPoseCurrent, const
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 both were found
if( itLandmarkQuery != m_cOptimizerSparse.vertices( ).end( ) &&
itLandmarkReference != m_cOptimizerSparse.vertices( ).end( ) )
//ds if query is in the graph
if( itLandmarkQuery != m_cOptimizerSparse.vertices( ).end( ) )
{
//ds set up the edge
g2o::EdgePointXYZ* pEdgeLandmarkClosure( new g2o::EdgePointXYZ( ) );
//ds consistency
assert( cMatch.cPointReference.uID == m_vecLandmarks->at( cMatch.cPointReference.uID )->uID );

//ds reference landmark
g2o::VertexPointXYZ* pVertexLandmarkReference = 0;

//ds check for reference landmark
if( itLandmarkReference != m_cOptimizerSparse.vertices( ).end( ) )
{
//ds get it from the graph
pVertexLandmarkReference = dynamic_cast< g2o::VertexPointXYZ* >( itLandmarkReference->second );
}
else
{
//ds create a new vertex
pVertexLandmarkReference = new g2o::VertexPointXYZ( );
pVertexLandmarkReference->setEstimate( m_vecLandmarks->at( cMatch.cPointReference.uID )->vecPointXYZOptimized );
pVertexLandmarkReference->setId( cMatch.cPointReference.uID );
m_cOptimizerSparse.addVertex( pVertexLandmarkReference );
}

//ds retrieve and fix reference landmark
g2o::VertexPointXYZ* 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, itLandmarkQuery->second );
pEdgeLandmarkClosure->setVertex( 1, pVertexLandmarkReference );
pEdgeLandmarkClosure->setMeasurement( Eigen::Vector3d::Zero( ) );
pEdgeLandmarkClosure->setInformation( m_matInformationLandmarkClosure );
pEdgeLandmarkClosure->setRobustKernel( new g2o::RobustKernelCauchy( ) );

//ds add to optimizer
m_cOptimizerSparse.addEdge( pEdgeLandmarkClosure );
Expand Down Expand Up @@ -718,29 +810,20 @@ void Cg2oOptimizer::_applyOptimization( const std::vector< CLandmark* >& p_vecCh

void Cg2oOptimizer::_applyOptimization( const std::vector< CKeyFrame* >& p_vecChunkKeyFrames )
{
//ds loop over all active poses
for( const g2o::VertexSE3* pVertexPose: m_vecActivePosesInGraph )
//ds update poses
for( CKeyFrame* pKeyFrame: p_vecChunkKeyFrames )
{
//ds get current id
const UIDKeyFrame uID = pVertexPose->id( )-m_uIDShift;
//ds find the corresponding landmark
const g2o::HyperGraph::VertexIDMap::iterator itPose( m_cOptimizerSparse.vertices( ).find( pKeyFrame->uID+m_uIDShift ) );

//ds id consistency enforced
assert( p_vecChunkKeyFrames[uID]->uID == uID );
//ds must be present
assert( itPose != m_cOptimizerSparse.vertices( ).end( ) );

//ds update keyframe
p_vecChunkKeyFrames[uID]->matTransformationLEFTtoWORLD = pVertexPose->estimate( );
p_vecChunkKeyFrames[uID]->bIsOptimized = true;
}
}
//ds get the pose from the graph
const g2o::VertexSE3* pVertexPose( dynamic_cast< g2o::VertexSE3* >( itPose->second ) );

void Cg2oOptimizer::_fixateTrajectory( )
{
//ds lock all active poses
for( g2o::VertexSE3* pVertexPose: m_vecActivePosesInGraph )
{
pVertexPose->setFixed( true );
//ds update keyframe
pKeyFrame->matTransformationLEFTtoWORLD = pVertexPose->estimate( );
pKeyFrame->bIsOptimized = true;
}

//ds clear active vector - all the poses were fixed and will not be changed by further optimizations
m_vecActivePosesInGraph.clear( );
}
Loading

0 comments on commit 6f2d88b

Please sign in to comment.