From e7cb35c8ba0331b2feaf04b05c3ae9b17e2d21cb Mon Sep 17 00:00:00 2001 From: aharmat Date: Thu, 12 Feb 2015 16:45:48 -0500 Subject: [PATCH] Implemented suggested bug fixes --- CMakeLists.txt | 25 +++++++----- cmake_modules/FindSUITESPARSE.cmake | 62 ++++++++++++++--------------- launch/mcptam.launch | 4 +- launch/pose_calibrator.launch | 6 +++ package.xml | 14 ++----- src/ChainBundle.cc | 4 +- 6 files changed, 57 insertions(+), 58 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index da959a8..09f1d8d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ set(CMAKE_BUILD_TYPE None) # Set our own specific flags. Want something like RelWithDebInfo but without the # -DNDEBUG flag that disables asserts -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++0x -march=core2 -msse3 -O1 -g") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++0x -march=core2 -msse3 -O3 -g -fopenmp") # Find various packages in order to set up environment variables find_package(catkin REQUIRED COMPONENTS roscpp image_transport sensor_msgs cv_bridge geometry_msgs tf message_filters std_msgs @@ -19,12 +19,13 @@ find_package(OpenCV REQUIRED) find_package(GLUT REQUIRED) find_package(G2O REQUIRED) find_package(GVars3 REQUIRED) -find_package(OpenCV REQUIRED) +find_package(BLAS REQUIRED) +find_package(LAPACK REQUIRED) include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${SUITESPARSE_INCLUDE_DIRS} ${CVD_INCLUDE_DIR} ${GVars3_INCLUDE_DIR} ${OpenCV_INCLUDE_DIRS}) -link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${SUITESPARSE_LIBRARY_DIRS} ${OpenCV_LIB_DIR}) +link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${SUITESPARSE_LIBRARY_DIRS} ${OpenCV_LIB_DIR} ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES}) add_message_files( DIRECTORY msg @@ -52,8 +53,8 @@ add_executable(mcptam src/Main.cc src/GLWindow2.cc src/GLWindowMenu.cc src/Video src/PatchFinder.cc src/Relocaliser.cc src/Tracker.cc src/KeyFrameViewer.cc src/BundleAdjusterBase.cc src/MiniPatch.cc src/BundleAdjusterMulti.cc src/ChainBundle.cc) add_dependencies(mcptam ${PROJECT_NAME}_gencpp) -target_link_libraries(mcptam ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${CVD_LIBRARY} ${OPENGL_gl_LIBRARY} ${GVars3_LIBRARIES} - ${G2O_STUFF_LIBRARY} ${G2O_CORE_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${OpenCV_LIBS}) +target_link_libraries(mcptam ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${CVD_LIBRARY} ${OPENGL_gl_LIBRARY} ${OpenCV_LIBS} ${GVars3_LIBRARIES} + ${G2O_STUFF_LIBRARY} ${G2O_CORE_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES}) add_executable(mcptam_client src/MainClient.cc src/GLWindow2.cc src/GLWindowMenu.cc src/VideoSourceMulti.cc src/CameraGroupSubscriber.cc src/SystemBase.cc src/SystemFrontendBase.cc src/SystemClient.cc src/TaylorCamera.cc src/KeyFrame.cc src/MapPoint.cc src/Map.cc @@ -61,7 +62,8 @@ add_executable(mcptam_client src/MainClient.cc src/GLWindow2.cc src/GLWindowMenu src/NetworkManager.cc src/PatchFinder.cc src/Relocaliser.cc src/MiniPatch.cc src/Tracker.cc src/KeyFrameViewer.cc) add_dependencies(mcptam_client ${PROJECT_NAME}_gencpp) -target_link_libraries(mcptam_client ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${CVD_LIBRARY} ${OPENGL_gl_LIBRARY} ${GVars3_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(mcptam_client ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${CVD_LIBRARY} ${OPENGL_gl_LIBRARY} ${OpenCV_LIBS} ${GVars3_LIBRARIES} + ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES}) add_executable(mcptam_server src/MainServer.cc src/GLWindow2.cc src/GLWindowMenu.cc src/VideoSourceMulti.cc src/CameraGroupSubscriber.cc src/SystemBase.cc src/SystemServer.cc src/TaylorCamera.cc src/KeyFrame.cc src/MapPoint.cc src/Map.cc @@ -69,13 +71,14 @@ add_executable(mcptam_server src/MainServer.cc src/GLWindow2.cc src/GLWindowMenu src/NetworkManager.cc src/PatchFinder.cc src/KeyFrameViewer.cc src/MiniPatch.cc src/BundleAdjusterBase.cc src/BundleAdjusterMulti.cc src/ChainBundle.cc) add_dependencies(mcptam_server ${PROJECT_NAME}_gencpp) -target_link_libraries(mcptam_server ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${CVD_LIBRARY} ${OPENGL_gl_LIBRARY} ${GVars3_LIBRARIES} - ${G2O_STUFF_LIBRARY} ${G2O_CORE_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${OpenCV_LIBS}) +target_link_libraries(mcptam_server ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${CVD_LIBRARY} ${OPENGL_gl_LIBRARY} ${OpenCV_LIBS} ${GVars3_LIBRARIES} + ${G2O_STUFF_LIBRARY} ${G2O_CORE_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES}) add_executable(camera_calibrator src/MainCameraCalibrator.cc src/GLWindow2.cc src/GLWindowMenu.cc src/VideoSourceSingle.cc src/CameraCalibrator.cc src/CalibImageTaylor.cc src/CalibCornerPatch.cc src/TaylorCamera.cc src/CalibGridCorner.cc) add_dependencies(camera_calibrator ${PROJECT_NAME}_gencpp) -target_link_libraries(camera_calibrator ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${CVD_LIBRARY} ${OPENGL_gl_LIBRARY} ${GVars3_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(camera_calibrator ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${CVD_LIBRARY} ${OPENGL_gl_LIBRARY} ${OpenCV_LIBS} ${GVars3_LIBRARIES} + ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES}) add_executable(pose_calibrator src/MainPoseCalibrator.cc src/GLWindow2.cc src/GLWindowMenu.cc src/VideoSourceMulti.cc src/CameraGroupSubscriber.cc src/SystemBase.cc src/PoseCalibrator.cc src/CalibImageTaylor.cc src/CalibCornerPatch.cc src/CalibGridCorner.cc @@ -84,8 +87,8 @@ add_executable(pose_calibrator src/MainPoseCalibrator.cc src/GLWindow2.cc src/GL src/PatchFinder.cc src/Relocaliser.cc src/Tracker.cc src/KeyFrameViewer.cc src/BundleAdjusterBase.cc src/BundleAdjusterSingle.cc src/BundleAdjusterCalib.cc src/ChainBundle.cc src/MiniPatch.cc) add_dependencies(pose_calibrator ${PROJECT_NAME}_gencpp) -target_link_libraries(pose_calibrator ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${CVD_LIBRARY} ${OPENGL_gl_LIBRARY} ${GVars3_LIBRARIES} - ${G2O_STUFF_LIBRARY} ${G2O_CORE_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${OpenCV_LIBS}) +target_link_libraries(pose_calibrator ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${CVD_LIBRARY} ${OPENGL_gl_LIBRARY} ${OpenCV_LIBS} ${GVars3_LIBRARIES} + ${G2O_STUFF_LIBRARY} ${G2O_CORE_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES}) install(TARGETS mcptam mcptam_client mcptam_server camera_calibrator pose_calibrator ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/cmake_modules/FindSUITESPARSE.cmake b/cmake_modules/FindSUITESPARSE.cmake index d6f0420..f30e078 100644 --- a/cmake_modules/FindSUITESPARSE.cmake +++ b/cmake_modules/FindSUITESPARSE.cmake @@ -38,18 +38,15 @@ if( WIN32 ) ENDIF( SUITESPARSE_LIBRARY_DIRS ) else( WIN32 ) - IF( APPLE) + IF(APPLE) FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h - PATHS /opt/local/include/ufsparse ) + PATHS /opt/local/include/ufsparse + /usr/local/include ) FIND_PATH( SUITESPARSE_LIBRARY_DIR - NAMES libSuiteSparse.dylib - PATHS /opt/local/lib ) - - list ( APPEND SUITESPARSE_LIBRARY_DIRS ${SUITESPARSE_LIBRARY_DIR} ) - - list ( APPEND SUITESPARSE_LIBRARIES SuiteSparse) - + NAMES libcholmod.a + PATHS /opt/local/lib + /usr/local/lib ) ELSE(APPLE) FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h PATHS /usr/local/include @@ -57,15 +54,14 @@ else( WIN32 ) /usr/include/suitesparse/ ${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod PATH_SUFFIXES cholmod/ CHOLMOD/ ) - FIND_PATH( SUITESPARSE_LIBRARY_DIR - NAMES libcholmod.so + NAMES libcholmod.so libcholmod.a PATHS /usr/lib - /usr/lib64 + /usr/lib64 + /usr/lib/x86_64-linux-gnu + /usr/lib/i386-linux-gnu /usr/local/lib ) - - ENDIF(APPLE) # Add cholmod include directory to collection include directories @@ -73,32 +69,31 @@ else( WIN32 ) list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} ) ENDIF( CHOLMOD_INCLUDE_DIR ) - # if we found the library, add it to the defined libraries IF ( SUITESPARSE_LIBRARY_DIR ) - # Skipped, as this is set for apple in the block above - if (NOT APPLE) - list ( APPEND SUITESPARSE_LIBRARIES amd) - list ( APPEND SUITESPARSE_LIBRARIES btf) - list ( APPEND SUITESPARSE_LIBRARIES camd) - list ( APPEND SUITESPARSE_LIBRARIES ccolamd) - list ( APPEND SUITESPARSE_LIBRARIES cholmod) - list ( APPEND SUITESPARSE_LIBRARIES colamd) - # list ( APPEND SUITESPARSE_LIBRARIES csparse) - list ( APPEND SUITESPARSE_LIBRARIES cxsparse) - list ( APPEND SUITESPARSE_LIBRARIES klu) - # list ( APPEND SUITESPARSE_LIBRARIES spqr) - list ( APPEND SUITESPARSE_LIBRARIES umfpack) - list ( APPEND SUITESPARSE_LIBRARIES lapack) - endif() - + list ( APPEND SUITESPARSE_LIBRARIES amd) + list ( APPEND SUITESPARSE_LIBRARIES btf) + list ( APPEND SUITESPARSE_LIBRARIES camd) + list ( APPEND SUITESPARSE_LIBRARIES ccolamd) + list ( APPEND SUITESPARSE_LIBRARIES cholmod) + list ( APPEND SUITESPARSE_LIBRARIES colamd) + # list ( APPEND SUITESPARSE_LIBRARIES csparse) + list ( APPEND SUITESPARSE_LIBRARIES cxsparse) + list ( APPEND SUITESPARSE_LIBRARIES klu) + # list ( APPEND SUITESPARSE_LIBRARIES spqr) + list ( APPEND SUITESPARSE_LIBRARIES umfpack) + + IF (APPLE) + list ( APPEND SUITESPARSE_LIBRARIES suitesparseconfig) + ENDIF (APPLE) + # Metis and spqr are optional FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY NAMES metis PATHS ${SUITESPARSE_LIBRARY_DIR} ) IF (SUITESPARSE_METIS_LIBRARY) - list ( APPEND SUITESPARSE_LIBRARIES ${SUITESPARSE_METIS_LIBRARY}) + list ( APPEND SUITESPARSE_LIBRARIES metis) ENDIF(SUITESPARSE_METIS_LIBRARY) if(EXISTS "${CHOLMOD_INCLUDE_DIR}/SuiteSparseQR.hpp") @@ -120,11 +115,14 @@ else( WIN32 ) endif( WIN32 ) + IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) IF(WIN32) list (APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR}/../../UFconfig ) ENDIF(WIN32) SET(SUITESPARSE_FOUND TRUE) + MESSAGE(STATUS "Found SuiteSparse") ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) SET( SUITESPARSE_FOUND FALSE ) + MESSAGE(FATAL_ERROR "Unable to find SuiteSparse") ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) diff --git a/launch/mcptam.launch b/launch/mcptam.launch index 969bc4e..4574cfc 100644 --- a/launch/mcptam.launch +++ b/launch/mcptam.launch @@ -21,8 +21,10 @@ --> + @@ -38,9 +40,7 @@ - diff --git a/launch/pose_calibrator.launch b/launch/pose_calibrator.launch index ccf0ed9..f4ef3d3 100644 --- a/launch/pose_calibrator.launch +++ b/launch/pose_calibrator.launch @@ -17,14 +17,20 @@ + + + diff --git a/package.xml b/package.xml index ab8d0b5..20d0881 100644 --- a/package.xml +++ b/package.xml @@ -1,13 +1,13 @@ mcptam - 0.1.1 + 0.1.2 MCPTAM Adam Harmat, Michael Tribou Adam Harmat - see http://www.robots.ox.ac.uk/~gk/PTAM/download.html - http://ros.org/wiki/mcptam + GPV v3 http://www.gnu.org/licenses/gpl-3.0.html + https://github.com/aharmat/mcptam/wiki catkin @@ -23,10 +23,6 @@ visualization_msgs dynamic_reconfigure pcl_ros - suitesparse - cvd - GVars3 - OpenGL message_generation libg2o @@ -42,10 +38,6 @@ visualization_msgs dynamic_reconfigure pcl_ros - suitesparse - cvd - GVars3 - OpenGL message_runtime libg2o diff --git a/src/ChainBundle.cc b/src/ChainBundle.cc index e7af267..778248a 100644 --- a/src/ChainBundle.cc +++ b/src/ChainBundle.cc @@ -436,11 +436,11 @@ class EdgeChainMeas : public g2o::BaseMultiEdge<2, TooN::Vector<2> > /* // Option 1: - std::vector > _jacobianOplusTemp(_vertices.size()); + std::vector > _jacobianOplusTemp(_vertices.size()); */ // Option 2: - std::vector >& _jacobianOplusTemp = _jacobianOplus; + std::vector >& _jacobianOplusTemp = _jacobianOplus; const VertexRelPoint* pPointVertex = dynamic_cast(_vertices.back()); ROS_ASSERT(pPointVertex);