Implemented suggested bug fixes

aharmat · Feb 12, 2015 · e7cb35c · e7cb35c
1 parent e898504
commit e7cb35c
Show file tree

Hide file tree

Showing 6 changed files with 57 additions and 58 deletions.
diff --git 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,30 +53,32 @@ 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 
                              src/SmallBlurryImage.cc src/ShiTomasi.cc src/MapMakerClient.cc src/MapMakerBase.cc src/MapMakerClientBase.cc 
                              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 
                              src/SmallBlurryImage.cc src/ShiTomasi.cc src/MapMakerServer.cc src/MapMakerBase.cc src/MapMakerServerBase.cc 
                              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
@@ -38,67 +38,62 @@ 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 
         	            /usr/include 
         	            /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
    IF ( CHOLMOD_INCLUDE_DIR )
 	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
@@ -21,8 +21,10 @@
   <param name="camera_pose_file" type="string" value="$(find mcptam)/poses/poses.dat" />
   -->
 
+<!--
   <rosparam command="load" file="$(find mcptam)/masks/masks_1,2,3,4.yaml" />
   <param name="masks_dir" type="string" value="$(find mcptam)/masks" />
+-->
 
   <param name="mm_init_point_mode" type="string" value="$(arg init_mode)" />
   <param name="mm_init_point_max_num" type="int" value="200" />
@@ -38,9 +40,7 @@
   <param name="headless" type="bool" value="$(arg headless)" />
 </node>
 
-<!--
 <node pkg="tf" type="static_transform_publisher" name="world_tf" args="0 0 0 0 0 3.14159 world vision_world 100" />
--->
 
 </launch>
 
diff --git a/launch/pose_calibrator.launch b/launch/pose_calibrator.launch
@@ -17,14 +17,20 @@
   <param name="kf_adaptive_thresh" type="bool" value="false" />
   <param name="mm_large_point_test" type="bool" value="false" />
 
+<!--
   <rosparam command="load" file="$(find mcptam)/masks/masks_1,2,3,4.yaml" />
   <param name="masks_dir" type="string" value="$(find mcptam)/masks" />
+-->
 
+<!--
   <param name="pose_out_file" type="string" value="$(find mcptam)/poses/poses.dat" /> 
+-->
 
 </node>
 
+<!--
 <node pkg="tf" type="static_transform_publisher" name="world_tf" args="0 0 0 -0.5 0.5 -0.5 0.5 world vision_world 100" />
+-->
 
 </launch>
 
diff --git a/package.xml b/package.xml
@@ -1,13 +1,13 @@
 <package>
   <name>mcptam</name>
-  <version>0.1.1</version>
+  <version>0.1.2</version>
   <description>
      MCPTAM
   </description>
   <author>Adam Harmat, Michael Tribou</author>
   <maintainer email="[email protected]">Adam Harmat</maintainer>
-  <license>see http://www.robots.ox.ac.uk/~gk/PTAM/download.html</license>
-  <url>http://ros.org/wiki/mcptam</url>
+  <license>GPV v3 http://www.gnu.org/licenses/gpl-3.0.html</license>
+  <url>https://github.com/aharmat/mcptam/wiki</url>
 
   <buildtool_depend>catkin</buildtool_depend>
 
@@ -23,10 +23,6 @@
   <build_depend>visualization_msgs</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>pcl_ros</build_depend>
-  <build_depend>suitesparse</build_depend>
-  <build_depend>cvd</build_depend>
-  <build_depend>GVars3</build_depend>
-  <build_depend>OpenGL</build_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>libg2o</build_depend>
 
@@ -42,10 +38,6 @@
   <run_depend>visualization_msgs</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>pcl_ros</run_depend>
-  <run_depend>suitesparse</run_depend>
-  <run_depend>cvd</run_depend>
-  <run_depend>GVars3</run_depend>
-  <run_depend>OpenGL</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>libg2o</run_depend>
 

diff --git 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<JacobianType, g2o::aligned_allocator<JacobianType> > _jacobianOplusTemp(_vertices.size());
+      std::vector<JacobianType, Eigen::aligned_allocator<JacobianType> > _jacobianOplusTemp(_vertices.size());
       */
 
       // Option 2:
-      std::vector<JacobianType, g2o::aligned_allocator<JacobianType> >& _jacobianOplusTemp = _jacobianOplus;
+      std::vector<JacobianType, Eigen::aligned_allocator<JacobianType> >& _jacobianOplusTemp = _jacobianOplus;
 
       const VertexRelPoint* pPointVertex = dynamic_cast<const VertexRelPoint*>(_vertices.back());
       ROS_ASSERT(pPointVertex);