Skip to content

Commit

Permalink
updated
Browse files Browse the repository at this point in the history
  • Loading branch information
@schdomin
schdomin committed Aug 3, 2015
1 parent d1cbb60 commit 4382068
Show file tree
Hide file tree
Showing 23 changed files with 675 additions and 437 deletions.
99 changes: 36 additions & 63 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,31 @@ src/vision/CMiniVisionToolbox.cpp
src/core/CMockedMatcherEpipolar.cpp
src/optimization/CBridgeG2O.cpp)

set(LIBRARIES_GENERAL
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${G2O_OPENGL_HELPER_LIBRARY}
${G2O_SOLVER_CSPARSE_EXTENSION}
${G2O_TYPES_SLAM3D}
${G2O_CORE_LIBRARY}
${G2O_STUFF_LIBRARY}
${G2O_TYPES_SBA}
${QGLVIEWER_LIBRARY}
${QT_QTXML_LIBRARY}
${QT_QTOPENGL_LIBRARY}
${QT_QTGUI_LIBRARY}
${QT_QTCORE_LIBRARY})

set(LIBRARIES_MOCKED
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${G2O_OPENGL_HELPER_LIBRARY}
${G2O_SOLVER_CSPARSE_EXTENSION}
${G2O_TYPES_SLAM3D}
${G2O_CORE_LIBRARY}
${G2O_STUFF_LIBRARY}
${G2O_TYPES_SBA})

#add_executable(message_dumper_node src/streaming/message_dumper_node.cpp)
#add_executable(stereo_detector_node src/analysis/stereo_detector_node.cpp)
#add_executable(stereo_detector_blocking_node src/analysis/stereo_detector_blocking_node.cpp)
Expand All @@ -79,73 +104,21 @@ src/optimization/CBridgeG2O.cpp)
#add_executable(stereo_detector_alberto_node src/runnable/stereo_detector_alberto_node.cpp src/core/CTrackerStereo.cpp ${SOURCEFILES})
add_executable(stereo_detector_mocked src/runnable/stereo_detector_mocked.cpp src/core/CMockedTrackerStereo.cpp ${SOURCEFILES_MOCKED})
#add_executable(test_depth src/runnable/test_depth.cpp ${SOURCEFILES})
add_executable(stereo_single src/runnable/stereo_single.cpp src/core/CTrackerStereoMotionModel.cpp ${SOURCEFILES})
add_executable(stereo_parallel src/runnable/stereo_parallel.cpp src/core/CTrackerStereoMotionModel.cpp ${SOURCEFILES})
#add_executable(stereo_single src/runnable/stereo_single.cpp src/core/CTrackerStereoMotionModel.cpp ${SOURCEFILES})
#add_executable(stereo_parallel src/runnable/stereo_parallel.cpp src/core/CTrackerStereoMotionModel.cpp ${SOURCEFILES})
add_executable(stereo_fps src/runnable/stereo_fps.cpp src/core/CTrackerStereoMotionModel.cpp ${SOURCEFILES})
#add_executable(republisher_node src/runnable/republisher_node.cpp ${SOURCEFILES})

#ds link the binaries against catkin
#target_link_libraries(message_dumper_node ${catkin_LIBRARIES} ${Boost_LIBRARIES})
#target_link_libraries(message_dumper_node ${catkin_LIBRARIES})
#target_link_libraries(stereo_detector_node ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
#target_link_libraries(stereo_detector_blocking_node ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

#target_link_libraries(stereo_detector_alberto_node
# ${catkin_LIBRARIES}
# ${OpenCV_LIBRARIES}
# ${G2O_OPENGL_HELPER_LIBRARY}
# ${G2O_SOLVER_CSPARSE_EXTENSION}
# ${G2O_TYPES_SLAM3D}
# ${G2O_CORE_LIBRARY}
# ${G2O_STUFF_LIBRARY}
# ${G2O_TYPES_SBA})
target_link_libraries(stereo_detector_mocked
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${G2O_OPENGL_HELPER_LIBRARY}
${G2O_SOLVER_CSPARSE_EXTENSION}
${G2O_TYPES_SLAM3D}
${G2O_CORE_LIBRARY}
${G2O_STUFF_LIBRARY}
${G2O_TYPES_SBA})
#target_link_libraries(test_depth
# ${catkin_LIBRARIES}
# ${OpenCV_LIBRARIES}
# -fopenmp)
target_link_libraries(stereo_single
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${G2O_OPENGL_HELPER_LIBRARY}
${G2O_SOLVER_CSPARSE_EXTENSION}
${G2O_TYPES_SLAM3D}
${G2O_CORE_LIBRARY}
${G2O_STUFF_LIBRARY}
${G2O_TYPES_SBA}
${QGLVIEWER_LIBRARY}
${QT_QTXML_LIBRARY}
${QT_QTOPENGL_LIBRARY}
${QT_QTGUI_LIBRARY}
${QT_QTCORE_LIBRARY} )
target_link_libraries(stereo_parallel
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${G2O_OPENGL_HELPER_LIBRARY}
${G2O_SOLVER_CSPARSE_EXTENSION}
${G2O_TYPES_SLAM3D}
${G2O_CORE_LIBRARY}
${G2O_STUFF_LIBRARY}
${G2O_TYPES_SBA}
${QGLVIEWER_LIBRARY}
${QT_QTXML_LIBRARY}
${QT_QTOPENGL_LIBRARY}
${QT_QTGUI_LIBRARY}
${QT_QTCORE_LIBRARY} )
#target_link_libraries(republisher_node
# ${catkin_LIBRARIES}
# ${OpenCV_LIBRARIES}
# ${G2O_OPENGL_HELPER_LIBRARY}
# ${G2O_SOLVER_CSPARSE_EXTENSION}
# ${G2O_TYPES_SLAM3D}
# ${G2O_CORE_LIBRARY}
# ${G2O_STUFF_LIBRARY}
# ${G2O_TYPES_SBA})
#target_link_libraries(stereo_detector_alberto_node ${LIBRARIES_GENERAL})
target_link_libraries(stereo_detector_mocked ${LIBRARIES_MOCKED})
#target_link_libraries(test_depth ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} -fopenmp)
#target_link_libraries(stereo_single ${LIBRARIES_GENERAL})
#target_link_libraries(stereo_parallel ${LIBRARIES_GENERAL})
target_link_libraries(stereo_fps ${LIBRARIES_GENERAL})
#target_link_libraries(republisher_node ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

message("--------------------------------------------------------------------")
2 changes: 1 addition & 1 deletion calibration_camera_0(RIGHT).txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ orientation:
z: -0.00862255915657
w: 0.999961080249

rotation:
rotation matrix:

0.99985 0.017244 0.00016324
-0.017245 0.99984 0.0037324
Expand Down
2 changes: 1 addition & 1 deletion calibration_camera_1(LEFT).txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ orientation:
z: -0.0114620263178
w: 0.999927556608

rotation:
rotation matrix:

0.9997325 0.0229121 0.0031568
-0.0229327 0.9997150 0.0066368
Expand Down
27 changes: 14 additions & 13 deletions src/configuration/CConfigurationCamera.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,8 @@ class CConfigurationCamera
static const double dFocalLengthMeters;
static const double matIntrinsic[9];
static const double vecDistortionCoefficients[4];
static const double matQuaternionFromIMU[4];
static const double vecTranslationFromIMU[3];
static const double matQuaternionToIMU[4];
static const double vecTranslationToIMU[3];
static const double matRectification[9];
static const double matProjection[12];

Expand All @@ -42,8 +42,8 @@ class CConfigurationCamera
static const double dFocalLengthMeters;
static const double matIntrinsic[9];
static const double vecDistortionCoefficients[4];
static const double matQuaternionFromIMU[4];
static const double vecTranslationFromIMU[3];
static const double matQuaternionToIMU[4];
static const double vecTranslationToIMU[3];
static const double matRectification[9];
static const double matProjection[12];

Expand All @@ -54,6 +54,7 @@ class CConfigurationCamera
static const double matTransformationIntialAlberto[16];
static const double matTransformationIntialStandard[16];
static const double matRotationIMU[9];
static const double matRotationToIMU[9];

};

Expand All @@ -68,12 +69,12 @@ const double CConfigurationCamera::LEFT::dFocalLengthMeters = 0.0028;

const double CConfigurationCamera::LEFT::matIntrinsic[] = {dFx, 0, dCx, 0, dFy, dCy, 0, 0, 1};
const double CConfigurationCamera::LEFT::vecDistortionCoefficients[] = {-0.2796582742482296, 0.0733556480116312, -0.0009278639320681781, -8.855407606759842e-06};
const double CConfigurationCamera::LEFT::matQuaternionFromIMU[] = {-0.00333631563313, 0.00154028789643, -0.0114620263178, 0.999927556608};
const double CConfigurationCamera::LEFT::vecTranslationFromIMU[] = {0.0666914200614, 0.0038316133947, -0.0101029245794};
const double CConfigurationCamera::LEFT::matQuaternionToIMU[] = {-0.00333631563313, 0.00154028789643, -0.0114620263178, 0.999927556608};
const double CConfigurationCamera::LEFT::vecTranslationToIMU[] = {0.0666914200614, 0.0038316133947, -0.0101029245794};
const double CConfigurationCamera::LEFT::matRectification[] = {0.9999606380973329, -0.005153089493488114, -0.0072227366452659965, 0.005142607646202396, 0.9999856976355481, -0.0014690510348766648, 0.007230203494507346, 0.0014318495095343982, 0.9999728366132802};
const double CConfigurationCamera::LEFT::matProjection[] = {450.5097158071153, 0.0, 375.9431800842285, 0.0, 0.0, 450.5097158071153, 222.3379611968994, 0.0, 0.0, 0.0, 1.0, 0.0};

const CPinholeCamera CConfigurationCamera::LEFT::cPinholeCamera = CPinholeCamera( "LEFT", matIntrinsic, vecDistortionCoefficients, matRectification, matProjection, matQuaternionFromIMU, vecTranslationFromIMU, uWidth, uHeight, dFocalLengthMeters );
const CPinholeCamera CConfigurationCamera::LEFT::cPinholeCamera = CPinholeCamera( "LEFT", matIntrinsic, vecDistortionCoefficients, matRectification, matProjection, matQuaternionToIMU, vecTranslationToIMU, matRotationIMU, uWidth, uHeight, dFocalLengthMeters );



Expand All @@ -88,12 +89,12 @@ const double CConfigurationCamera::RIGHT::dFocalLengthMeters = 0.0028;

const double CConfigurationCamera::RIGHT::matIntrinsic[] = {dFx, 0, dCx, 0, dFy, dCy, 0, 0, 1};
const double CConfigurationCamera::RIGHT::vecDistortionCoefficients[] = {-0.2786354177535735, 0.07230699700225292, -0.0003789301271210703, 8.083561308516752e-05};
const double CConfigurationCamera::RIGHT::matQuaternionFromIMU[] = {-0.00186686047363, 6.55239757426e-05, -0.00862255915657, 0.999961080249};
const double CConfigurationCamera::RIGHT::vecTranslationFromIMU[] = {-0.0434705406089, 0.00417949317011, -0.00942355850866};
const double CConfigurationCamera::RIGHT::matQuaternionToIMU[] = {-0.00186686047363, 6.55239757426e-05, -0.00862255915657, 0.999961080249};
const double CConfigurationCamera::RIGHT::vecTranslationToIMU[] = {-0.0434705406089, 0.00417949317011, -0.00942355850866};
const double CConfigurationCamera::RIGHT::matRectification[] = {0.999990753148139, 0.0005481502260720453, -0.004265342840574327, -0.0005419629065900193, 0.9999987995316526, 0.001451623732757562, 0.004266133428042508, -0.0014492986522044342, 0.9999898497679818};
const double CConfigurationCamera::RIGHT::matProjection[] = {450.5097158071153, 0.0, 375.9431800842285, -49.63250853439215, 0.0, 450.5097158071153, 222.3379611968994, 0.0, 0.0, 0.0, 1.0, 0.0};

const CPinholeCamera CConfigurationCamera::RIGHT::cPinholeCamera = CPinholeCamera( "RIGHT", matIntrinsic, vecDistortionCoefficients, matRectification, matProjection, matQuaternionFromIMU, vecTranslationFromIMU, uWidth, uHeight, dFocalLengthMeters );
const CPinholeCamera CConfigurationCamera::RIGHT::cPinholeCamera = CPinholeCamera( "RIGHT", matIntrinsic, vecDistortionCoefficients, matRectification, matProjection, matQuaternionToIMU, vecTranslationToIMU, matRotationIMU, uWidth, uHeight, dFocalLengthMeters );

const double CConfigurationCamera::matTransformationIntialAlberto[] = { 0.994504, 0.0803288, 0.0671501, -3.14989,
0.052872, 0.168259, -0.984324, 0.0468936,
Expand All @@ -105,8 +106,8 @@ const double CConfigurationCamera::matTransformationIntialStandard[] = { 1, 0,
0, 1, 0, 0,
0, 0, 0, 1 };

const double CConfigurationCamera::matRotationIMU[] = { 1, 0, 0,
0, 0, -1,
0, 1, 0 };
const double CConfigurationCamera::matRotationIMU[] = { 1, 0, 0,
0, -1, 0,
0, 0, -1 };

#endif //#define CCONFIGURATIONCAMERA_H_
84 changes: 35 additions & 49 deletions src/core/CMockedMatcherEpipolar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,55 +38,12 @@ CMockedMatcherEpipolar::~CMockedMatcherEpipolar( )

void CMockedMatcherEpipolar::addMeasurementPoint( const Eigen::Isometry3d& p_matTransformationLEFTtoWORLD, const std::shared_ptr< std::vector< CLandmark* > > p_vecLandmarks )
{
m_vecLandmarkCreationPointsActive.push_back( CLandmarkCreationPoint( m_uAvailableMeasurementPointID, p_matTransformationLEFTtoWORLD, p_vecLandmarks ) );
m_vecDetectionPointsActive.push_back( CDetectionPoint( m_uAvailableMeasurementPointID, p_matTransformationLEFTtoWORLD, p_vecLandmarks ) );

++m_uAvailableMeasurementPointID;
}

const Eigen::Isometry3d CMockedMatcherEpipolar::getPoseOptimized( const Eigen::Isometry3d& p_matTransformationEstimateWORLDtoLEFT )
{
//ds optimized pose
Eigen::Isometry3d matTransformationOptimizedWORLDtoLEFT( p_matTransformationEstimateWORLDtoLEFT );

//ds vectors for pose solver
gtools::Vector3dVector vecLandmarksWORLD;
gtools::Vector2dVector vecImagePoints;

//ds active measurements
for( const CLandmarkCreationPoint cCreationPoint: m_vecLandmarkCreationPointsActive )
{
//ds loop over the points for the current scan
for( const CLandmark* pLandmarkReference: *cCreationPoint.vecLandmarks )
{
//ds world position
const CPoint3DInWorldFrame& vecPointXYZ( pLandmarkReference->vecPointXYZOptimized );

//ds store world position
vecLandmarksWORLD.push_back( vecPointXYZ );

//ds project point into current scenery
vecImagePoints.push_back( m_pCameraLEFT->getUV( p_matTransformationEstimateWORLDtoLEFT*vecPointXYZ ) );
}
}

//ds feed the solver with the 3D points (in camera frame)
m_cSolverPose.model_points = vecLandmarksWORLD;

//ds feed the solver with the 2D points
m_cSolverPose.image_points = vecImagePoints;

//ds initial guess of the transformation
m_cSolverPose.T = p_matTransformationEstimateWORLDtoLEFT;

//double dErrorSolverPosePrevious( 0.0 );
//const double dDeltaConvergence( 1e-5 );

m_cSolverPose.init( );

return matTransformationOptimizedWORLDtoLEFT;
}

const std::shared_ptr< std::vector< const CMeasurementLandmark* > > CMockedMatcherEpipolar::getVisibleLandmarksEssential( const uint64_t p_uFrame,
const std::shared_ptr< std::vector< const CMeasurementLandmark* > > CMockedMatcherEpipolar::getVisibleLandmarks( const uint64_t p_uFrame,
cv::Mat& p_matDisplayLEFT,
cv::Mat& p_matDisplayRIGHT,
const cv::Mat& p_matImageLEFT,
Expand All @@ -109,15 +66,15 @@ const std::shared_ptr< std::vector< const CMeasurementLandmark* > > CMockedMatch
const std::map< UIDLandmark, CMockedDetection > mapVisibleLandmarks( m_pCameraSTEREO->getDetectedLandmarks( ptPositionXY, matTransformationWORLDtoLEFT, p_matDisplayTrajectory ) );

//ds new active measurement points after this matching
std::vector< CLandmarkCreationPoint > vecMeasurementPointsActive;
std::vector< CDetectionPoint > vecMeasurementPointsActive;

//ds vectors for pose solver
gtools::Vector3dVector vecLandmarksWORLD;
gtools::Vector2dVector vecImagePointsLEFT;
gtools::Vector2dVector vecImagePointsRIGHT;

//ds active measurements
for( const CLandmarkCreationPoint cMeasurementPoint: m_vecLandmarkCreationPointsActive )
for( const CDetectionPoint cMeasurementPoint: m_vecDetectionPointsActive )
{
//ds visible (=in this image detected) active (=not detected in this image but failed detections below threshold)
std::vector< const CMeasurementLandmark* > vecVisibleLandmarksPerMeasurementPoint;
Expand All @@ -144,6 +101,7 @@ const std::shared_ptr< std::vector< const CMeasurementLandmark* > > CMockedMatch
m_pCameraRIGHT->m_matProjection ) );

//ds update landmark
pLandmarkReference->bIsCurrentlyVisible = true;
pLandmarkReference->matDescriptorLASTLEFT = CDescriptor( );
pLandmarkReference->matDescriptorLASTRIGHT = CDescriptor( );
pLandmarkReference->uFailedSubsequentTrackings = 0;
Expand Down Expand Up @@ -194,7 +152,7 @@ const std::shared_ptr< std::vector< const CMeasurementLandmark* > > CMockedMatch
if( !vecActiveLandmarksPerMeasurementPoint->empty( ) )
{
//ds register the measurement point and its visible landmarks anew
vecMeasurementPointsActive.push_back( CLandmarkCreationPoint( cMeasurementPoint.uID, cMeasurementPoint.matTransformationLEFTtoWORLD, vecActiveLandmarksPerMeasurementPoint ) );
vecMeasurementPointsActive.push_back( CDetectionPoint( cMeasurementPoint.uID, cMeasurementPoint.matTransformationLEFTtoWORLD, vecActiveLandmarksPerMeasurementPoint ) );

//ds combine visible landmarks
vecVisibleLandmarks->insert( vecVisibleLandmarks->end( ), vecVisibleLandmarksPerMeasurementPoint.begin( ), vecVisibleLandmarksPerMeasurementPoint.end( ) );
Expand Down Expand Up @@ -279,8 +237,36 @@ const std::shared_ptr< std::vector< const CMeasurementLandmark* > > CMockedMatch
}

//ds update active measurement points
m_vecLandmarkCreationPointsActive.swap( vecMeasurementPointsActive );
m_vecDetectionPointsActive.swap( vecMeasurementPointsActive );

//ds return active landmarks
return vecVisibleLandmarks;
}

//ds routine that resets the visibility of all active landmarks
void CMockedMatcherEpipolar::resetVisibilityActiveLandmarks( )
{
//ds active measurements
for( const CDetectionPoint& cDetectionPoint: m_vecDetectionPointsActive )
{
//ds loop over the points for the current scan
for( CLandmark* pLandmark: *cDetectionPoint.vecLandmarks )
{
pLandmark->bIsCurrentlyVisible = false;
}
}
}

void CMockedMatcherEpipolar::setKeyFrameToVisibleLandmarks( )
{
//ds active measurements
for( const CDetectionPoint& cDetectionPoint: m_vecDetectionPointsActive )
{
//ds loop over the points for the current scan
for( CLandmark* pLandmark: *cDetectionPoint.vecLandmarks )
{
//ds added in keyframe
if( pLandmark->bIsCurrentlyVisible ){ ++pLandmark->uNumberOfKeyFramePresences; }
}
}
}
Loading

0 comments on commit 4382068

Please sign in to comment.