first commit

CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+
+SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
+find_package(Eigen3 REQUIRED)
+include_directories(${EIGEN3_INCLUDE_DIR})
+include_directories("${PROJECT_SOURCE_DIR}/include")
+
+add_subdirectory(isamlib)
+link_libraries(isamlib)
+
+rosbuild_add_executable(featureTracking src/featureTracking.cpp)
+rosbuild_add_executable(visualOdometry src/visualOdometry.cpp)
+rosbuild_add_executable(bundleAdjust src/bundleAdjust.cpp)
+rosbuild_add_executable(processDepthmap src/processDepthmap.cpp)
+rosbuild_add_executable(stackDepthPoint src/stackDepthPoint.cpp)
+rosbuild_add_executable(transformMaintenance src/transformMaintenance.cpp)
+rosbuild_add_executable(registerPointCloud src/registerPointCloud.cpp)
+
+target_link_libraries(bundleAdjust ${EIGEN3_INCLUDE_DIR})

Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

cmake/FindCholmod.cmake

@@ -0,0 +1,79 @@
+# Cholmod lib usually requires linking to a blas and lapack library.
+# It is up to the user of this module to find a BLAS and link to it.
+
+if (CHOLMOD_INCLUDES AND CHOLMOD_LIBRARIES)
+  set(CHOLMOD_FIND_QUIETLY TRUE)
+endif (CHOLMOD_INCLUDES AND CHOLMOD_LIBRARIES)
+
+find_path(CHOLMOD_INCLUDES
+  NAMES
+  cholmod.h
+  PATHS
+  $ENV{CHOLMODDIR}
+  ${INCLUDE_INSTALL_DIR}
+  PATH_SUFFIXES
+  suitesparse
+)
+
+find_library(CHOLMOD_LIBRARIES cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+
+if(CHOLMOD_LIBRARIES)
+
+  get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH)
+
+  find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (AMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (COLAMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CAMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CCOLAMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CHOLMOD_METIS_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY})
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(CHOLMOD DEFAULT_MSG
+                                  CHOLMOD_INCLUDES CHOLMOD_LIBRARIES)
+
+mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY)

cmake/FindEigen3.cmake

@@ -0,0 +1,81 @@
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+#   find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <[email protected]>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <[email protected]>
+# Copyright (c) 2009 Benoit Jacob <[email protected]>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+  if(NOT Eigen3_FIND_VERSION_MAJOR)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
+  endif(NOT Eigen3_FIND_VERSION_MAJOR)
+  if(NOT Eigen3_FIND_VERSION_MINOR)
+    set(Eigen3_FIND_VERSION_MINOR 91)
+  endif(NOT Eigen3_FIND_VERSION_MINOR)
+  if(NOT Eigen3_FIND_VERSION_PATCH)
+    set(Eigen3_FIND_VERSION_PATCH 0)
+  endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK FALSE)
+  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK TRUE)
+  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+  if(NOT EIGEN3_VERSION_OK)
+
+    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+                   "but at least version ${Eigen3_FIND_VERSION} is required")
+  endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+      PATHS
+      ${CMAKE_INSTALL_PREFIX}/include
+      ${KDE4_INCLUDE_DIR}
+      PATH_SUFFIXES eigen3 eigen
+    )
+
+  if(EIGEN3_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif(EIGEN3_INCLUDE_DIR)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+  mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
+

demo_rgbd.launch

@@ -0,0 +1,13 @@
+<launch>
+
+  <node pkg="demo_rgbd" type="featureTracking" name="featureTracking" output="screen"/>
+  <node pkg="demo_rgbd" type="visualOdometry" name="visualOdometry" output="screen"/>
+  <node pkg="demo_rgbd" type="bundleAdjust" name="bundleAdjust" output="screen"/>
+  <node pkg="demo_rgbd" type="processDepthmap" name="processDepthmap" output="screen"/>
+  <node pkg="demo_rgbd" type="stackDepthPoint" name="stackDepthPoint" output="screen"/>
+  <node pkg="demo_rgbd" type="transformMaintenance" name="transformMaintenance" output="screen"/>
+  <node pkg="demo_rgbd" type="registerPointCloud" name="registerPointCloud" output="screen"/>
+
+  <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find demo_rgbd)/demo_rgbd.vcg"/>
+
+</launch>

demo_rgbd.vcg

@@ -0,0 +1,160 @@
+AfterMapped.Enabled=0
+AfterMapped.Length=0.5
+AfterMapped.Radius=0.12
+AfterMapped.Reference\ Frame=/aft_ba
+Background\ ColorB=0
+Background\ ColorG=0
+Background\ ColorR=0
+Camera\ Config=-0.264797 1.58041 28.744 0.275569 -0.0088194 6.87178
+Camera\ Type=rviz::OrbitViewController
+Camera2.Enabled=1
+Camera2.Length=1
+Camera2.Radius=0.1
+Camera2.Reference\ Frame=/camera2
+Fixed\ Frame=/camera_init
+Image.Enabled=1
+Image.Image\ Topic=/image/show_2
+Image.Queue\ Size=2
+Image.Transport\ Hint=raw
+Odometry.Angle\ Tolerance=0.1
+Odometry.ColorB=0
+Odometry.ColorG=0.1
+Odometry.ColorR=1
+Odometry.Enabled=1
+Odometry.Keep=1000
+Odometry.Length=1
+Odometry.Position\ Tolerance=0.1
+Odometry.Topic=/cam2_to_init
+Property\ Grid\ Splitter=901,0
+Property\ Grid\ State=expanded=.Global Options;splitterratio=0.5
+QMainWindow=000000ff00000000fd000000030000000000000118000004bbfc0200000002fb000000100044006900730070006c006100790073010000002d000003ce000000c300fffffffb0000000a0049006d0061006700650100000401000000e70000001400ffffff000000010000013100000477fc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000002d000004770000005100fffffffb0000000a0056006900650077007302000002ae000000000000013100000400fb0000001200530065006c0065006300740069006f006e000000001d0000039b0000005100ffffff00000003000004cf0000003efc0100000001fb0000000800540069006d00650000000000000004cf0000021400ffffff000002b1000004bb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+Target\ Frame=/camera_init
+Tool\ 2D\ Nav\ GoalTopic=/move_base_simple/goal
+Tool\ 2D\ Pose\ EstimateTopic=initialpose
+camera.Enabled=0
+camera.Length=1
+camera.Radius=0.1
+camera.Reference\ Frame=/camera
+cameraInit.Enabled=0
+cameraInit.Length=1.5
+cameraInit.Radius=0.1
+cameraInit.Reference\ Frame=/camera_init
+depthCloud..AxisColorAutocompute\ Value\ Bounds=1
+depthCloud..AxisColorAxis=2
+depthCloud..AxisColorMax\ Value=14.7891
+depthCloud..AxisColorMin\ Value=8.66777
+depthCloud..AxisColorUse\ Fixed\ Frame=1
+depthCloud..FlatColorColorB=0
+depthCloud..FlatColorColorG=1
+depthCloud..FlatColorColorR=1
+depthCloud..IntensityAutocompute\ Intensity\ Bounds=1
+depthCloud..IntensityChannel\ Name=intensity
+depthCloud..IntensityMax\ ColorB=1
+depthCloud..IntensityMax\ ColorG=1
+depthCloud..IntensityMax\ ColorR=1
+depthCloud..IntensityMax\ Intensity=4096
+depthCloud..IntensityMin\ ColorB=0
+depthCloud..IntensityMin\ ColorG=0
+depthCloud..IntensityMin\ ColorR=0
+depthCloud..IntensityMin\ Intensity=0
+depthCloud..IntensityUse\ full\ RGB\ spectrum=0
+depthCloud.Alpha=1
+depthCloud.Billboard\ Size=0.02
+depthCloud.Color\ Transformer=FlatColor
+depthCloud.Decay\ Time=0
+depthCloud.Enabled=0
+depthCloud.Position\ Transformer=XYZ
+depthCloud.Queue\ Size=10
+depthCloud.Selectable=1
+depthCloud.Style=1
+depthCloud.Topic=/depth_cloud
+imageCloud..AxisColorAutocompute\ Value\ Bounds=1
+imageCloud..AxisColorAxis=2
+imageCloud..AxisColorMax\ Value=4.61162
+imageCloud..AxisColorMin\ Value=1.1223
+imageCloud..AxisColorUse\ Fixed\ Frame=1
+imageCloud..FlatColorColorB=1
+imageCloud..FlatColorColorG=1
+imageCloud..FlatColorColorR=1
+imageCloud..IntensityAutocompute\ Intensity\ Bounds=1
+imageCloud..IntensityChannel\ Name=intensity
+imageCloud..IntensityMax\ ColorB=1
+imageCloud..IntensityMax\ ColorG=1
+imageCloud..IntensityMax\ ColorR=1
+imageCloud..IntensityMax\ Intensity=4096
+imageCloud..IntensityMin\ ColorB=0
+imageCloud..IntensityMin\ ColorG=0
+imageCloud..IntensityMin\ ColorR=0
+imageCloud..IntensityMin\ Intensity=0
+imageCloud..IntensityUse\ full\ RGB\ spectrum=0
+imageCloud.Alpha=1
+imageCloud.Billboard\ Size=0.05
+imageCloud.Color\ Transformer=FlatColor
+imageCloud.Decay\ Time=0
+imageCloud.Enabled=1
+imageCloud.Position\ Transformer=XYZ
+imageCloud.Queue\ Size=10
+imageCloud.Selectable=1
+imageCloud.Style=1
+imageCloud.Topic=/image_points_proj
+surroundCloud..AxisColorAutocompute\ Value\ Bounds=1
+surroundCloud..AxisColorAxis=2
+surroundCloud..AxisColorMax\ Value=18.1132
+surroundCloud..AxisColorMin\ Value=2.1672
+surroundCloud..AxisColorUse\ Fixed\ Frame=1
+surroundCloud..FlatColorColorB=1
+surroundCloud..FlatColorColorG=1
+surroundCloud..FlatColorColorR=1
+surroundCloud..IntensityAutocompute\ Intensity\ Bounds=1
+surroundCloud..IntensityChannel\ Name=intensity
+surroundCloud..IntensityMax\ ColorB=1
+surroundCloud..IntensityMax\ ColorG=1
+surroundCloud..IntensityMax\ ColorR=1
+surroundCloud..IntensityMax\ Intensity=10
+surroundCloud..IntensityMin\ ColorB=0
+surroundCloud..IntensityMin\ ColorG=0
+surroundCloud..IntensityMin\ ColorR=0
+surroundCloud..IntensityMin\ Intensity=10
+surroundCloud..IntensityUse\ full\ RGB\ spectrum=0
+surroundCloud.Alpha=1
+surroundCloud.Billboard\ Size=0.02
+surroundCloud.Color\ Transformer=AxisColor
+surroundCloud.Decay\ Time=0
+surroundCloud.Enabled=1
+surroundCloud.Position\ Transformer=XYZ
+surroundCloud.Queue\ Size=10
+surroundCloud.Selectable=1
+surroundCloud.Style=1
+surroundCloud.Topic=/surround_cloud
+[Display0]
+ClassName=rviz::ImageDisplay
+Name=Image
+[Display1]
+ClassName=rviz::PointCloud2Display
+Name=depthCloud
+[Display2]
+ClassName=rviz::PointCloud2Display
+Name=imageCloud
+[Display3]
+ClassName=rviz::PointCloud2Display
+Name=surroundCloud
+[Display4]
+ClassName=rviz::AxesDisplay
+Name=cameraInit
+[Display5]
+ClassName=rviz::AxesDisplay
+Name=camera
+[Display6]
+ClassName=rviz::AxesDisplay
+Name=Camera2
+[Display7]
+ClassName=rviz::AxesDisplay
+Name=AfterMapped
+[Display8]
+ClassName=rviz::OdometryDisplay
+Name=Odometry
+[Window]
+Height=1256
+Width=975
+X=-940
+Y=-1204