diff --git a/CMakeLists.txt b/CMakeLists.txt
index 337fd19a4..cf5a1a962 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -46,9 +46,7 @@ endif()
 # Set up options
 
 # Configurable Options
-option(GTSAM_BUILD_TESTS                 "Enable/Disable building of tests"          ON)
 option(GTSAM_BUILD_TIMING                "Enable/Disable building of timing scripts" OFF) # These do not currently work
-option(GTSAM_BUILD_EXAMPLES              "Enable/Disable building of examples"       ON)
 if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          ON)
 endif()
@@ -83,18 +81,6 @@ endif()
 set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
 set(CPACK_GENERATOR        "TGZ" CACHE STRING "CPack Default Binary Generator")
 
-# Flags to determine whether tests and examples are build during 'make install'
-# Note that these remove the targets from the 'all'
-option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON)
-option(GTSAM_DISABLE_EXAMPLES_ON_INSTALL "Disables building examples during install" OFF)
-
-# Pull in infrastructure
-if (GTSAM_BUILD_TESTS)
-    enable_testing()
-    include(Dart)
-    include(CTest)
-endif()
-
 ###############################################################################
 # Find boost
 
@@ -132,6 +118,7 @@ else()
     if(Boost_TIMER_LIBRARY)
       list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY})
     else()
+      list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt
       message("WARNING:  GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.")
     endif()
 endif()
@@ -364,19 +351,17 @@ set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>=
 message(STATUS "===============================================================")
 message(STATUS "================  Configuration Options  ======================")
 message(STATUS "Build flags                                               ")
-print_config_flag(${GTSAM_BUILD_TIMING}                "Build Timing scripts           ")
-print_config_flag(${GTSAM_BUILD_EXAMPLES}              "Build Examples                 ")
 print_config_flag(${GTSAM_BUILD_TESTS}                 "Build Tests                    ")
+print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS}       "Build examples with 'make all' ")
+print_config_flag(${GTSAM_BUILD_TIMING}                "Build Timing scripts           ")
 if (DOXYGEN_FOUND)
-    print_config_flag(${GTSAM_BUILD_DOCS}                  "Build Docs                     ")
+    print_config_flag(${GTSAM_BUILD_DOCS}              "Build Docs                     ")
 endif()
 print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY}        "Build static GTSAM library instead of shared")
 print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES}        "Put build type in library name ")
 if(GTSAM_UNSTABLE_AVAILABLE)
-    print_config_flag(${GTSAM_BUILD_UNSTABLE}           "Build libgtsam_unstable        ")
+    print_config_flag(${GTSAM_BUILD_UNSTABLE}          "Build libgtsam_unstable        ")
 endif()
-print_config_flag(${GTSAM_DISABLE_TESTS_ON_INSTALL}     "No tests in all or install     ")
-print_config_flag(${GTSAM_DISABLE_EXAMPLES_ON_INSTALL}  "No examples in all or install  ")
 string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper)
 if(NOT MSVC AND NOT XCODE_VERSION)
     message(STATUS "  Build type                     : ${CMAKE_BUILD_TYPE}")
diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index f00f3b3b1..51e66627d 100644
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@@ -102,7 +102,7 @@ if(    NOT cmake_build_type_tolower STREQUAL ""
 endif()
 
 # Mark that first pass is done
-set(FIRST_PASS_DONE TRUE CACHE BOOL "Internally used to mark whether cmake has been run multiple times" FORCE)
+set(FIRST_PASS_DONE TRUE CACHE INTERNAL "Internally used to mark whether cmake has been run multiple times")
 mark_as_advanced(FIRST_PASS_DONE) 
 
 # Enable Visual Studio solution folders
diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake
index d699795fd..a8592f16c 100644
--- a/cmake/GtsamMatlabWrap.cmake
+++ b/cmake/GtsamMatlabWrap.cmake
@@ -48,7 +48,7 @@ set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e
 #   extraIncludeDirs : Extra include directories, in addition to those already passed to include_directories(...)
 #   extraMexFlags    : Any additional compiler flags
 function(wrap_and_install_library interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)
-	wrap_library_internal("${interfaceHeader}" "${otherLibraries}" "${extraIncludeDirs}" "${mexFlags}")
+	wrap_library_internal("${interfaceHeader}" "${linkLibraries}" "${extraIncludeDirs}" "${mexFlags}")
 	install_wrapped_library_internal("${interfaceHeader}")
 endfunction()
 
@@ -103,7 +103,8 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
 	# Add -shared or -static suffix to targets
 	set(correctedOtherLibraries "")
 	set(otherLibraryTargets "")
-	foreach(lib ${moduleName} ${otherLibraries})
+    set(otherLibraryNontargets "")
+	foreach(lib ${moduleName} ${linkLibraries})
 		if(TARGET ${lib})
 			list(APPEND correctedOtherLibraries ${lib})
 			list(APPEND otherLibraryTargets ${lib})
@@ -115,8 +116,20 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
 			list(APPEND otherLibraryTargets ${lib}-static)
 		else()
 			list(APPEND correctedOtherLibraries ${lib})
+            list(APPEND otherLibraryNontargets ${lib})
 		endif()
 	endforeach()
+    
+    # Check libraries for conflicting versions built-in to MATLAB
+    set(dependentLibraries "")
+    if(NOT "${otherLibraryTargets}" STREQUAL "")
+        foreach(target ${otherLibraryTargets})
+            get_target_property(dependentLibrariesOne ${target} INTERFACE_LINK_LIBRARIES)
+            list(APPEND dependentLibraries ${dependentLibrariesOne})
+        endforeach()
+    endif()
+    list(APPEND dependentLibraries ${otherLibraryNontargets})
+    check_conflicting_libraries_internal("${dependentLibraries}")
 
 	# Set up generation of module source file
 	file(MAKE_DIRECTORY "${generated_files_path}")
@@ -205,6 +218,76 @@ function(install_wrapped_library_internal interfaceHeader)
 	endif()
 endfunction()
 
+function(check_conflicting_libraries_internal libraries)
+    if(UNIX)
+        # Set path for matlab's built-in libraries
+        if(APPLE)
+            set(mxLibPath "${MATLAB_ROOT}/bin/maci64")
+        else()
+            if(CMAKE_CL_64)
+                set(mxLibPath "${MATLAB_ROOT}/bin/glnxa64")
+            else()
+                set(mxLibPath "${MATLAB_ROOT}/bin/glnx86")
+            endif()
+        endif()
+        
+        # List matlab's built-in libraries
+        file(GLOB matlabLibs RELATIVE "${mxLibPath}" "${mxLibPath}/lib*")
+        
+        # Convert to base names
+        set(matlabLibNames "")
+        foreach(lib ${matlabLibs})
+            get_filename_component(libName "${lib}" NAME_WE)
+            list(APPEND matlabLibNames "${libName}")
+        endforeach()
+        
+        # Get names of link libraries
+        set(linkLibNames "")
+        foreach(lib ${libraries})
+            string(FIND "${lib}" "/" slashPos)
+            if(NOT slashPos EQUAL -1)
+                # If the name is a path, just get the library name
+                get_filename_component(libName "${lib}" NAME_WE)
+                list(APPEND linkLibNames "${libName}")
+            else()
+                # It's not a path, so see if it looks like a filename
+                get_filename_component(ext "${lib}" EXT)
+                if(NOT "${ext}" STREQUAL "")
+                    # It's a filename, so get the base name
+                    get_filename_component(libName "${lib}" NAME_WE)
+                    list(APPEND linkLibNames "${libName}")
+                else()
+                    # It's not a filename so it must be a short name, add the "lib" prefix
+                    list(APPEND linkLibNames "lib${lib}")
+                endif()
+            endif()
+        endforeach()
+        
+        # Remove duplicates
+        list(REMOVE_DUPLICATES linkLibNames)
+        
+        set(conflictingLibs "")
+        foreach(lib ${linkLibNames})
+            list(FIND matlabLibNames "${lib}" libPos)
+            if(NOT libPos EQUAL -1)
+                if(NOT conflictingLibs STREQUAL "")
+                    set(conflictingLibs "${conflictingLibs}, ")
+                endif()
+                set(conflictingLibs "${conflictingLibs}${lib}")
+            endif()
+        endforeach()
+        
+        if(NOT "${conflictingLibs}" STREQUAL "")
+            message(WARNING "GTSAM links to the libraries [ ${conflictingLibs} ] on your system, but "
+                "MATLAB is distributed with its own versions of these libraries which may conflict. "
+                "If you get strange errors or crashes with the GTSAM MATLAB wrapper, move these "
+                "libraries out of MATLAB's built-in library directory, which is ${mxLibPath} on "
+                "your system.  MATLAB will usually still work with these libraries moved away, but "
+                "if not, you'll have to compile the static GTSAM MATLAB wrapper module.")
+        endif()
+    endif()
+endfunction()
+
 
 # Function to setup codegen and building of the wrap toolbox
 #
diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake
index f1b7c5e54..bd8aba766 100644
--- a/cmake/GtsamTesting.cmake
+++ b/cmake/GtsamTesting.cmake
@@ -1,111 +1,204 @@
-# Build macros for using tests
+# This file defines the two macros below for easily adding groups of unit tests and scripts,
+# as well as sets up unit testing and defines several cache options used to control how
+# tests and scripts are built and run.
 
+
+
+###############################################################################
+# Macro:
+#
+# gtsamAddTestsGlob(groupName globPatterns excludedFiles linkLibraries)
+# 
+# Add a group of unit tests.  A list of unit test .cpp files or glob patterns specifies the
+# tests to create.  Tests are assigned into a group name so they can easily by run
+# independently with a make target.  Running 'make check' builds and runs all tests.
+#
+# Usage example:
+#   gtsamAddTestsGlob(basic "test*.cpp" "testBroken.cpp" "gtsam;GeographicLib")
+#
+# Arguments:
+#   groupName:     A name that will allow this group of tests to be run independently, e.g.
+#                  'basic' causes a 'check.basic' target to be created to run this test
+#                  group.
+#   globPatterns:  The list of files or glob patterns from which to create unit tests, with
+#                  one test created for each cpp file.  e.g. "test*.cpp", or
+#                  "testA*.cpp;testB*.cpp;testOneThing.cpp".
+#   excludedFiles: A list of files or globs to exclude, e.g. "testC*.cpp;testBroken.cpp".
+#                  Pass an empty string "" if nothing needs to be excluded.
+#   linkLibraries: The list of libraries to link to in addition to CppUnitLite.
+macro(gtsamAddTestsGlob groupName globPatterns excludedFiles linkLibraries)
+	gtsamAddTestsGlob_impl("${groupName}" "${globPatterns}" "${excludedFiles}" "${linkLibraries}")
+endmacro()
+
+
+###############################################################################
+# Macro:
+#
+# gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)
+# 
+# Add scripts that will serve as examples of how to use the library.  A list of files or
+# glob patterns is specified, and one executable will be created for each matching .cpp
+# file.  These executables will not be installed.  They are build with 'make all' if
+# GTSAM_BUILD_EXAMPLES_ALWAYS is enabled.  They may also be built with 'make examples'.
+#
+# Usage example:
+#   gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
+#
+# Arguments:
+#   globPatterns:  The list of files or glob patterns from which to create unit tests, with
+#                  one test created for each cpp file.  e.g. "*.cpp", or
+#                  "A*.cpp;B*.cpp;MyExample.cpp".
+#   excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp".  Pass
+#                  an empty string "" if nothing needs to be excluded.
+#   linkLibraries: The list of libraries to link to.
+macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries)
+	gtsamAddExamplesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}")
+endmacro()
+
+
+# Implementation follows:
+
+# Build macros for using tests
 enable_testing()
 
-# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
-add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
-add_custom_target(timing)
+option(GTSAM_BUILD_TESTS                 "Enable/Disable building of tests"          ON)
+option(GTSAM_BUILD_EXAMPLES_ALWAYS       "Build examples with 'make all' (build with 'make examples' if not)"       ON)
 
 # Add option for combining unit tests
-if(MSVC)
+if(MSVC OR XCODE_VERSION)
 	option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON)
 else()
 	option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF)
 endif()
+mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
 
-# Macro for adding categorized tests in a "tests" folder, with 
-# optional exclusion of tests and convenience library linking options
-#  
-# By default, all tests are linked with CppUnitLite and boost
-# Arguments: 
-#   - subdir    The name of the category for this test
-#   - local_libs  A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
-#   - full_libs   The main library to link against if not using convenience libraries
-#   - excluded_tests  A list of test files that should not be compiled - use for debugging 
-function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests)
-    # Subdirectory target for tests
-    add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
-    set(is_test TRUE)
+# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
+if(GTSAM_BUILD_TESTS)
+    add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
+endif()
 
-	# Put check target in Visual Studio solution folder
-	file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
-	set_property(TARGET check.${subdir} PROPERTY FOLDER "${relative_path}")
-    
-	# Link with CppUnitLite - pulled from gtsam installation
-	list(APPEND local_libs CppUnitLite)
-	list(APPEND full_libs CppUnitLite)
+# Add examples target
+add_custom_target(examples)
 
-    # Build grouped tests
-    gtsam_add_grouped_scripts("${subdir}"               # Use subdirectory as group label
-    "tests/test*.cpp" check "Test"                      # Standard for all tests
-    "${local_libs}"
-    "${full_libs}" "${excluded_tests}"  # Pass in linking and exclusion lists
-    ${is_test})                                         # Set all as tests
-endfunction()
+# Include obsolete macros - will be removed in the near future
+include(GtsamTestingObsolete)
 
-# Macro for adding categorized timing scripts in a "tests" folder, with 
-# optional exclusion of tests and convenience library linking options
-#  
-# By default, all tests are linked with boost
-# Arguments: 
-#   - subdir    The name of the category for this timing script
-#   - local_libs  A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
-#   - full_libs   The main library to link against if not using convenience libraries
-#   - excluded_srcs  A list of timing files that should not be compiled - use for debugging 
-macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs)
-    # Subdirectory target for timing - does not actually execute the scripts
-    add_custom_target(timing.${subdir})
-    set(is_test FALSE)
 
-    # Build grouped benchmarks
-    gtsam_add_grouped_scripts("${subdir}"               # Use subdirectory as group label
-    "tests/time*.cpp" timing "Timing Benchmark"         # Standard for all timing scripts
-    "${local_libs}" "${full_libs}" "${excluded_srcs}"   # Pass in linking and exclusion lists
-    ${is_test})                                         # Treat as not a test
+# Implementations of this file's macros:
+
+macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
+	if(GTSAM_BUILD_TESTS)
+		# Add group target if it doesn't already exist
+	    if(NOT TARGET check.${groupName})
+			add_custom_target(check.${groupName} COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
+		endif()
+	
+	    # Get all script files
+        file(GLOB script_files ${globPatterns})
+
+	    # Remove excluded scripts from the list
+	    if(NOT "${excludedFiles}" STREQUAL "")
+			file(GLOB excludedFilePaths ${excludedFiles})
+			if("${excludedFilePaths}" STREQUAL "")
+				message(WARNING "The pattern '${excludedFiles}' for excluding tests from group ${groupName} did not match any files")
+			else()
+		    	list(REMOVE_ITEM script_files ${excludedFilePaths})
+			endif()
+	    endif()
+	
+		# Separate into source files and headers (allows for adding headers to show up in
+		# MSVC and Xcode projects).
+		set(script_srcs "")
+		set(script_headers "")
+		foreach(script_file IN ITEMS ${script_files})
+			get_filename_component(script_ext ${script_file} EXT)
+			if(script_ext MATCHES "(h|H)")
+				list(APPEND script_headers ${script_file})
+			else()
+				list(APPEND script_srcs ${script_file})
+			endif()
+		endforeach()
+	
+		# Don't put test files in folders in MSVC and Xcode because they're already grouped
+		source_group("" FILES ${script_srcs} ${script_headers})
+
+		if(NOT GTSAM_SINGLE_TEST_EXE)
+			# Default for Makefiles - each test in its own executable
+			foreach(script_src IN ITEMS ${script_srcs})
+				# Get test base name
+				get_filename_component(script_name ${script_src} NAME_WE)
+			
+				# Add executable
+				add_executable(${script_name} ${script_src} ${script_headers})
+				target_link_libraries(${script_name} CppUnitLite ${linkLibraries})
+				
+				# Add target dependencies
+				add_test(NAME ${script_name} COMMAND ${script_name})
+				add_dependencies(check.${groupName} ${script_name})
+				add_dependencies(check ${script_name})
+				if(NOT MSVC AND NOT XCODE_VERSION)
+				  add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
+				endif()
+			
+				# Add TOPSRCDIR
+				set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
+			
+				# Exclude from 'make all' and 'make install'
+				set_target_properties(${script_name} PROPERTIES EXCLUDE_FROM_ALL ON)
+			
+				# Configure target folder (for MSVC and Xcode)
+				set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests/${groupName}")
+			endforeach()
+		else()
+			# Default on MSVC and XCode - combine test group into a single exectuable
+			set(target_name check_${groupName}_program)
+		
+			# Add executable
+			add_executable(${target_name} ${script_srcs} ${script_headers})
+			target_link_libraries(${target_name} CppUnitLite ${linkLibraries})
+		
+			# Only have a main function in one script - use preprocessor
+			set(rest_script_srcs ${script_srcs})
+			list(REMOVE_AT rest_script_srcs 0)
+			set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main")
+		
+			# Add target dependencies
+			add_test(NAME ${target_name} COMMAND ${target_name})
+			add_dependencies(check.${groupName} ${target_name})
+			add_dependencies(check ${target_name})
+		
+			# Add TOPSRCDIR
+			set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
+		
+			# Exclude from 'make all' and 'make install'
+			set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON)
+
+			# Configure target folder (for MSVC and Xcode)
+			set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests")
+		endif()
+	endif()
 endmacro()
 
-# Macro for adding executables matching a pattern - builds one executable for
-# each file matching the pattern.  These exectuables are automatically linked
-# with boost.
-# Arguments:
-#   - pattern    The glob pattern to match source files
-#   - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
-#   - full_libs  The main library to link against if not using convenience libraries
-#   - excluded_srcs  A list of timing files that should not be compiled - use for debugging
-function(gtsam_add_executables pattern local_libs full_libs excluded_srcs)
-    set(is_test FALSE)
-    
-    if(NOT excluded_srcs)
-        set(excluded_srcs "")
-    endif()
-    
-    # Build executables
-    gtsam_add_grouped_scripts("" "${pattern}" "" "Executable" "${local_libs}" "${full_libs}" "${excluded_srcs}" ${is_test})
-endfunction()
 
-# General-purpose script for adding tests with categories and linking options
-macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name local_libs full_libs excluded_srcs is_test) 
+macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries)
     # Get all script files
-    set(script_files "")
-    foreach(one_pattern ${pattern})
-        file(GLOB one_script_files "${one_pattern}")
-        list(APPEND script_files "${one_script_files}")
-    endforeach()
+    file(GLOB script_files ${globPatterns})
 
     # Remove excluded scripts from the list
-    set(exclusions "") # Need to copy out exclusion list for logic to work
-    foreach(one_exclusion ${excluded_srcs})
-        file(GLOB one_exclusion_srcs "${one_exclusion}")
-        list(APPEND exclusions "${one_exclusion_srcs}")
-    endforeach()
-    if(exclusions)
-    	list(REMOVE_ITEM script_files ${exclusions})
-    endif(exclusions)
+    if(NOT "${excludedFiles}" STREQUAL "")
+		file(GLOB excludedFilePaths ${excludedFiles})
+		if("${excludedFilePaths}" STREQUAL "")
+			message(WARNING "The script exclusion pattern '${excludedFiles}' did not match any files")
+		else()
+	    	list(REMOVE_ITEM script_files ${excludedFilePaths})
+		endif()
+    endif()
 
-	# Separate into source files and headers
+	# Separate into source files and headers (allows for adding headers to show up in
+	# MSVC and Xcode projects).
 	set(script_srcs "")
 	set(script_headers "")
-	foreach(script_file ${script_files})
+	foreach(script_file IN ITEMS ${script_files})
 		get_filename_component(script_ext ${script_file} EXT)
 		if(script_ext MATCHES "(h|H)")
 			list(APPEND script_headers ${script_file})
@@ -113,94 +206,34 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
 			list(APPEND script_srcs ${script_file})
 		endif()
 	endforeach()
-		
-    
-    # Add targets and dependencies for each script
-    if(NOT "${group}" STREQUAL "")
-        message(STATUS "Adding ${pretty_prefix_name}s in ${group}")
-    endif()
-	
-	# Create exe's for each script, unless we're in SINGLE_TEST_EXE mode
-	if(NOT is_test OR NOT GTSAM_SINGLE_TEST_EXE)
-		foreach(script_src ${script_srcs})
-			get_filename_component(script_base ${script_src} NAME_WE)
-			if (script_base) # Check for null filenames and headers
-				set( script_bin ${script_base} )
-				message(STATUS "Adding ${pretty_prefix_name} ${script_bin}") 
-				add_executable(${script_bin} ${script_src} ${script_headers})
-				if(NOT "${target_prefix}" STREQUAL "")
-					if(NOT "${group}" STREQUAL "")
-						add_dependencies(${target_prefix}.${group} ${script_bin})
-					endif()
-					add_dependencies(${target_prefix} ${script_bin})
-				endif()
-				
-				# Add TOPSRCDIR
-				set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
 
-				# Disable building during make all/install
-				if (GTSAM_DISABLE_TESTS_ON_INSTALL)
-					set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
-				endif()
-				
-				if (is_test)
-					add_test(NAME ${script_base} COMMAND ${script_bin})
-				endif()
-				
-				# Linking and dependendencies
-				if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-					target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES})
-				else()
-					target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES})
-				endif()
-				
-				# Add .run target
-				if(NOT MSVC AND NOT XCODE_VERSION)
-				  add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN})
-				endif()
-				
-				# Set up Visual Studio folders
-				file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
-				set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
-			endif()
-		endforeach(script_src)
-		
-		if(MSVC)
-			source_group("" FILES ${script_srcs} ${script_headers})
+	# Don't put test files in folders in MSVC and Xcode because they're already grouped
+	source_group("" FILES ${script_srcs} ${script_headers})
+
+	# Create executables
+	foreach(script_src IN ITEMS ${script_srcs})
+		# Get script base name
+		get_filename_component(script_name ${script_src} NAME_WE)
+
+		# Add executable
+		add_executable(${script_name} ${script_src} ${script_headers})
+		target_link_libraries(${script_name} ${linkLibraries})
+	
+		# Add target dependencies
+		add_dependencies(examples ${script_name})
+		if(NOT MSVC AND NOT XCODE_VERSION)
+		  add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
 		endif()
-	else()
-		# Create single unit test exe from all test scripts
-		set(script_bin ${target_prefix}_${group}_prog)
-		add_executable(${script_bin} ${script_srcs} ${script_headers})
-		if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-			target_link_libraries(${script_bin} ${local_libs} ${Boost_LIBRARIES})
-		else()
-			target_link_libraries(${script_bin} ${Boost_LIBRARIES} ${full_libs})
-		endif()
-		
-		# Only have a main function in one script
-		set(rest_script_srcs ${script_srcs})
-		list(REMOVE_AT rest_script_srcs 0)
-		set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main")
-			
+
 		# Add TOPSRCDIR
-		set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
-			
-		# Add test
-		add_dependencies(${target_prefix}.${group} ${script_bin})
-		add_dependencies(${target_prefix} ${script_bin})
-		add_test(NAME ${target_prefix}.${group} COMMAND ${script_bin})
-		
-		# Disable building during make all/install
-		if (GTSAM_DISABLE_TESTS_ON_INSTALL)
-			set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
+		set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
+	
+		if(NOT GTSAM_BUILD_EXAMPLES_ALWAYS)
+			# Exclude from 'make all' and 'make install'
+			set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON)
 		endif()
-		
-		# Set up Visual Studio folders
-		if(MSVC)
-		    file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
-			set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
-			source_group("" FILES ${script_srcs} ${script_headers})
-		endif()
-	endif()
+
+		# Configure target folder (for MSVC and Xcode)
+		set_property(TARGET ${script_name} PROPERTY FOLDER "Examples")
+	endforeach()
 endmacro()
diff --git a/cmake/GtsamTestingObsolete.cmake b/cmake/GtsamTestingObsolete.cmake
new file mode 100644
index 000000000..f56d138e6
--- /dev/null
+++ b/cmake/GtsamTestingObsolete.cmake
@@ -0,0 +1,195 @@
+
+# Macro for adding categorized tests in a "tests" folder, with 
+# optional exclusion of tests and convenience library linking options
+#  
+# By default, all tests are linked with CppUnitLite and boost
+# Arguments: 
+#   - subdir    The name of the category for this test
+#   - local_libs  A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
+#   - full_libs   The main library to link against if not using convenience libraries
+#   - excluded_tests  A list of test files that should not be compiled - use for debugging 
+function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests)
+    # Subdirectory target for tests
+    add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
+    set(is_test TRUE)
+
+	# Put check target in Visual Studio solution folder
+	file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
+	set_property(TARGET check.${subdir} PROPERTY FOLDER "${relative_path}")
+    
+	# Link with CppUnitLite - pulled from gtsam installation
+	list(APPEND local_libs CppUnitLite)
+	list(APPEND full_libs CppUnitLite)
+
+    # Build grouped tests
+    gtsam_add_grouped_scripts("${subdir}"               # Use subdirectory as group label
+    "tests/test*.cpp" check "Test"                      # Standard for all tests
+    "${local_libs}"
+    "${full_libs}" "${excluded_tests}"  # Pass in linking and exclusion lists
+    ${is_test})                                         # Set all as tests
+endfunction()
+
+# Macro for adding categorized timing scripts in a "tests" folder, with 
+# optional exclusion of tests and convenience library linking options
+#  
+# By default, all tests are linked with boost
+# Arguments: 
+#   - subdir    The name of the category for this timing script
+#   - local_libs  A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
+#   - full_libs   The main library to link against if not using convenience libraries
+#   - excluded_srcs  A list of timing files that should not be compiled - use for debugging 
+macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs)
+    # Subdirectory target for timing - does not actually execute the scripts
+    add_custom_target(timing.${subdir})
+    set(is_test FALSE)
+
+    # Build grouped benchmarks
+    gtsam_add_grouped_scripts("${subdir}"               # Use subdirectory as group label
+    "tests/time*.cpp" timing "Timing Benchmark"         # Standard for all timing scripts
+    "${local_libs}" "${full_libs}" "${excluded_srcs}"   # Pass in linking and exclusion lists
+    ${is_test})                                         # Treat as not a test
+endmacro()
+
+# Macro for adding executables matching a pattern - builds one executable for
+# each file matching the pattern.  These exectuables are automatically linked
+# with boost.
+# Arguments:
+#   - pattern    The glob pattern to match source files
+#   - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
+#   - full_libs  The main library to link against if not using convenience libraries
+#   - excluded_srcs  A list of timing files that should not be compiled - use for debugging
+function(gtsam_add_executables pattern local_libs full_libs excluded_srcs)
+    set(is_test FALSE)
+    
+    if(NOT excluded_srcs)
+        set(excluded_srcs "")
+    endif()
+    
+    # Build executables
+    gtsam_add_grouped_scripts("" "${pattern}" "" "Executable" "${local_libs}" "${full_libs}" "${excluded_srcs}" ${is_test})
+endfunction()
+
+# General-purpose script for adding tests with categories and linking options
+macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name local_libs full_libs excluded_srcs is_test)
+	# Print warning about using this obsolete function
+	message(AUTHOR_WARNING "Warning:  Please see GtsamTesting.cmake - obsolete cmake cmake macro for creating unit tests, examples, and scripts was called.  This will be removed in the future.  The new macros are much easier anyway!!")
+	
+    # Get all script files
+    set(script_files "")
+    foreach(one_pattern ${pattern})
+        file(GLOB one_script_files "${one_pattern}")
+        list(APPEND script_files "${one_script_files}")
+    endforeach()
+
+    # Remove excluded scripts from the list
+    set(exclusions "") # Need to copy out exclusion list for logic to work
+    foreach(one_exclusion ${excluded_srcs})
+        file(GLOB one_exclusion_srcs "${one_exclusion}")
+        list(APPEND exclusions "${one_exclusion_srcs}")
+    endforeach()
+    if(exclusions)
+    	list(REMOVE_ITEM script_files ${exclusions})
+    endif(exclusions)
+
+	# Separate into source files and headers
+	set(script_srcs "")
+	set(script_headers "")
+	foreach(script_file ${script_files})
+		get_filename_component(script_ext ${script_file} EXT)
+		if(script_ext MATCHES "(h|H)")
+			list(APPEND script_headers ${script_file})
+		else()
+			list(APPEND script_srcs ${script_file})
+		endif()
+	endforeach()
+		
+    
+    # Add targets and dependencies for each script
+    if(NOT "${group}" STREQUAL "")
+        message(STATUS "Adding ${pretty_prefix_name}s in ${group}")
+    endif()
+	
+	# Create exe's for each script, unless we're in SINGLE_TEST_EXE mode
+	if(NOT is_test OR NOT GTSAM_SINGLE_TEST_EXE)
+		foreach(script_src ${script_srcs})
+			get_filename_component(script_base ${script_src} NAME_WE)
+			if (script_base) # Check for null filenames and headers
+				set( script_bin ${script_base} )
+				message(STATUS "Adding ${pretty_prefix_name} ${script_bin}") 
+				add_executable(${script_bin} ${script_src} ${script_headers})
+				if(NOT "${target_prefix}" STREQUAL "")
+					if(NOT "${group}" STREQUAL "")
+						add_dependencies(${target_prefix}.${group} ${script_bin})
+					endif()
+					add_dependencies(${target_prefix} ${script_bin})
+				endif()
+				
+				# Add TOPSRCDIR
+				set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
+
+				# Disable building during make all/install
+				if (GTSAM_DISABLE_TESTS_ON_INSTALL)
+					set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
+				endif()
+				
+				if (is_test)
+					add_test(NAME ${script_base} COMMAND ${script_bin})
+				endif()
+				
+				# Linking and dependendencies
+				if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+					target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES})
+				else()
+					target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES})
+				endif()
+				
+				# Add .run target
+				if(NOT MSVC AND NOT XCODE_VERSION)
+				  add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN})
+				endif()
+				
+				# Set up Visual Studio folders
+				file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
+				set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
+			endif()
+		endforeach(script_src)
+		
+		if(MSVC)
+			source_group("" FILES ${script_srcs} ${script_headers})
+		endif()
+	else()
+		# Create single unit test exe from all test scripts
+		set(script_bin ${target_prefix}_${group}_prog)
+		add_executable(${script_bin} ${script_srcs} ${script_headers})
+		if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
+			target_link_libraries(${script_bin} ${local_libs} ${Boost_LIBRARIES})
+		else()
+			target_link_libraries(${script_bin} ${Boost_LIBRARIES} ${full_libs})
+		endif()
+		
+		# Only have a main function in one script
+		set(rest_script_srcs ${script_srcs})
+		list(REMOVE_AT rest_script_srcs 0)
+		set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main")
+			
+		# Add TOPSRCDIR
+		set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
+			
+		# Add test
+		add_dependencies(${target_prefix}.${group} ${script_bin})
+		add_dependencies(${target_prefix} ${script_bin})
+		add_test(NAME ${target_prefix}.${group} COMMAND ${script_bin})
+		
+		# Disable building during make all/install
+		if (GTSAM_DISABLE_TESTS_ON_INSTALL)
+			set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
+		endif()
+		
+		# Set up Visual Studio folders
+		if(MSVC)
+		    file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
+			set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
+			source_group("" FILES ${script_srcs} ${script_headers})
+		endif()
+	endif()
+endmacro()
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index d87070330..7251c2b6f 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -1,42 +1,8 @@
-if(NOT MSVC)
-  add_custom_target(examples)
-endif()
-
-# Build example executables
-FILE(GLOB example_srcs "*.cpp")
-
-set (excluded_examples #"")
-    "${CMAKE_CURRENT_SOURCE_DIR}/DiscreteBayesNet_FG.cpp"
-    "${CMAKE_CURRENT_SOURCE_DIR}/UGM_chain.cpp"
-    "${CMAKE_CURRENT_SOURCE_DIR}/UGM_small.cpp"
-    "${CMAKE_CURRENT_SOURCE_DIR}/elaboratePoint2KalmanFilter.cpp"
+set (excluded_examples
+    DiscreteBayesNet_FG.cpp
+    UGM_chain.cpp
+    UGM_small.cpp
+    elaboratePoint2KalmanFilter.cpp
 )
 
-list(REMOVE_ITEM example_srcs ${excluded_examples})
-
-foreach(example_src ${example_srcs} )
-    get_filename_component(example_base ${example_src} NAME_WE)
-    set( example_bin ${example_base} )
-    message(STATUS "Adding Example ${example_bin}")
-	if(NOT MSVC)
-      add_dependencies(examples ${example_bin})
-	endif()
-    add_executable(${example_bin} ${example_src})
-    
-    # Disable building during make all/install
-    if (GTSAM_DISABLE_EXAMPLES_ON_INSTALL)
-        set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
-    endif()
-    
-    target_link_libraries(${example_bin} gtsam ${Boost_PROGRAM_OPTIONS_LIBRARY})
-	if(NOT MSVC AND NOT XCODE_VERSION)
-      add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN})
-	endif()
-	
-	# Set up Visual Studio folder
-	if(MSVC)
-	  set_property(TARGET ${example_bin} PROPERTY FOLDER "Examples")
-	endif()
-
-endforeach(example_src)
-
+gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}")
diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp
index 7fbbb9ac1..ec980b960 100644
--- a/examples/SolverComparer.cpp
+++ b/examples/SolverComparer.cpp
@@ -536,8 +536,8 @@ void runCompare()
 void runPerturb()
 {
   // Set up random number generator
-  boost::random::mt19937 rng;
-  boost::random::normal_distribution<double> normal(0.0, perturbationNoise);
+  boost::mt19937 rng;
+  boost::normal_distribution<double> normal(0.0, perturbationNoise);
 
   // Perturb values
   VectorValues noise;
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 946ec5bc6..9fac3b00b 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -156,5 +156,5 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
 	endif()
 
     # Wrap
-    wrap_and_install_library(../gtsam.h "gtsam;${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
+    wrap_and_install_library(../gtsam.h "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
 endif ()
diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt
index 94e2473ed..50145846e 100644
--- a/gtsam/base/CMakeLists.txt
+++ b/gtsam/base/CMakeLists.txt
@@ -5,16 +5,8 @@ install(FILES ${base_headers} DESTINATION include/gtsam/base)
 file(GLOB base_headers_tree "treeTraversal/*.h")
 install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal)
 
-# Files to exclude from compilation of tests and timing scripts
-set(base_excluded_files
-# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test
-#    ""  # Add to this list, with full path, to exclude
-)
-
 # Build tests
-if (GTSAM_BUILD_TESTS)
-    gtsam_add_subdir_tests(base "gtsam" "gtsam" "${base_excluded_files}") 
-endif(GTSAM_BUILD_TESTS)
+add_subdirectory(tests)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h
index 14014898a..45edbf6af 100644
--- a/gtsam/base/SymmetricBlockMatrix.h
+++ b/gtsam/base/SymmetricBlockMatrix.h
@@ -61,32 +61,32 @@ namespace gtsam {
 
     /// Construct from a container of the sizes of each block.
     template<typename CONTAINER>
-    SymmetricBlockMatrix(const CONTAINER& dimensions) :
+    SymmetricBlockMatrix(const CONTAINER& dimensions, bool appendOneDimension = false) :
       blockStart_(0)
     {
-      fillOffsets(dimensions.begin(), dimensions.end());
+      fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
       matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
       assertInvariants();
     }
 
     /// Construct from iterator over the sizes of each vertical block.
     template<typename ITERATOR>
-    SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) :
+    SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension = false) :
       blockStart_(0)
     {
-      fillOffsets(firstBlockDim, lastBlockDim);
+      fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
       matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
       assertInvariants();
     }
 
     /// Construct from a container of the sizes of each vertical block and a pre-prepared matrix.
     template<typename CONTAINER>
-    SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
+    SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
       blockStart_(0)
     {
       matrix_.resize(matrix.rows(), matrix.cols());
       matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
-      fillOffsets(dimensions.begin(), dimensions.end());
+      fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
       if(matrix_.rows() != matrix_.cols())
         throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix.");
       if(variableColOffsets_.back() != matrix_.cols())
@@ -211,85 +211,21 @@ namespace gtsam {
       return variableColOffsets_[block + blockStart_];
     }
 
-    //void checkRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
-    //{
-    //  const DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
-    //  const DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
-    //  const DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
-    //  const DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
-    //  checkBlock(i_actualStartBlock);
-    //  checkBlock(j_actualStartBlock);
-    //  if(i_startBlock != 0 || i_endBlock != 0) {
-    //    checkBlock(i_actualStartBlock);
-    //    assert(i_actualEndBlock < (DenseIndex)variableColOffsets_.size());
-    //  }
-    //  if(j_startBlock != 0 || j_endBlock != 0) {
-    //    checkBlock(j_actualStartBlock);
-    //    assert(j_actualEndBlock < (DenseIndex)variableColOffsets_.size());
-    //  }
-    //}
-
-    //void checkRange(DenseIndex startBlock, DenseIndex endBlock) const
-    //{
-    //  const DenseIndex actualStartBlock = startBlock + blockStart_;
-    //  const DenseIndex actualEndBlock = endBlock + blockStart_;
-    //  checkBlock(actualStartBlock);
-    //  if(startBlock != 0 || endBlock != 0) {
-    //    checkBlock(actualStartBlock);
-    //    assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
-    //  }
-    //}
-
-    //Block rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock)
-    //{
-    //  const DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
-    //  const DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
-    //  const DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
-    //  const DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
-
-    //  return Block(matrix(),
-    //    variableColOffsets_[i_actualStartBlock],
-    //    variableColOffsets_[j_actualStartBlock],
-    //    variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock],
-    //    variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]);
-    //}
-
-    //constBlock rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
-    //{
-    //  // Convert Block to constBlock
-    //  const Block block = const_cast<This*>(this)->rangeUnchecked(i_startBlock, i_endBlock, j_startBlock, j_endBlock);
-    //  return constBlock(matrix(), block.Base::Base::, block.startCol(), block.rows(), block.cols());
-    //}
-
-    //Block rangeUnchecked(DenseIndex startBlock, DenseIndex endBlock)
-    //{
-    //  const DenseIndex actualStartBlock = startBlock + blockStart_;
-    //  const DenseIndex actualEndBlock = endBlock + blockStart_;
-
-    //  return Block(matrix(),
-    //    variableColOffsets_[actualStartBlock],
-    //    variableColOffsets_[actualStartBlock],
-    //    variableColOffsets_[actualEndBlock] - variableColOffsets_[actualStartBlock],
-    //    variableColOffsets_[actualEndBlock] - variableColOffsets_[actualStartBlock]);
-    //}
-
-    //constBlock rangeUnchecked(DenseIndex startBlock, DenseIndex endBlock) const
-    //{
-    //  // Convert Block to constBlock
-    //  const Block block = const_cast<This*>(this)->rangeUnchecked(startBlock, endBlock);
-    //  return constBlock(matrix(), block.startRow(), block.startCol(), block.rows(), block.cols());
-    //}
-
     template<typename ITERATOR>
-    void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim)
+    void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension)
     {
-      variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
+      variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
       variableColOffsets_[0] = 0;
       DenseIndex j=0;
       for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
         variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
         ++ j;
       }
+      if(appendOneDimension)
+      {
+        variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
+        ++ j;
+      }
     }
 
     friend class VerticalBlockMatrix;
diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h
index 0e45a8f0d..029f55c58 100644
--- a/gtsam/base/VerticalBlockMatrix.h
+++ b/gtsam/base/VerticalBlockMatrix.h
@@ -65,20 +65,20 @@ namespace gtsam {
 
     /** Construct from a container of the sizes of each vertical block. */
     template<typename CONTAINER>
-    VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height) :
+    VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) :
       rowStart_(0), rowEnd_(height), blockStart_(0)
     {
-      fillOffsets(dimensions.begin(), dimensions.end());
+      fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
       matrix_.resize(height, variableColOffsets_.back());
       assertInvariants();
     }
 
     /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
     template<typename CONTAINER>
-    VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
+    VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
       matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0)
     {
-      fillOffsets(dimensions.begin(), dimensions.end());
+      fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
       if(variableColOffsets_.back() != matrix_.cols())
         throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
       assertInvariants();
@@ -87,10 +87,10 @@ namespace gtsam {
     /**
      * Construct from iterator over the sizes of each vertical block. */
     template<typename ITERATOR>
-    VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) :
+    VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) :
       rowStart_(0), rowEnd_(height), blockStart_(0)
     {
-      fillOffsets(firstBlockDim, lastBlockDim);
+      fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
       matrix_.resize(height, variableColOffsets_.back());
       assertInvariants();
     }
@@ -202,14 +202,19 @@ namespace gtsam {
     }
 
     template<typename ITERATOR>
-    void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
-      variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
+    void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) {
+      variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
       variableColOffsets_[0] = 0;
       DenseIndex j=0;
       for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
         variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
         ++ j;
       }
+      if(appendOneDimension)
+      {
+        variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
+        ++ j;
+      }
     }
 
     friend class SymmetricBlockMatrix;
diff --git a/gtsam/base/tests/CMakeLists.txt b/gtsam/base/tests/CMakeLists.txt
new file mode 100644
index 000000000..8e99ef5ba
--- /dev/null
+++ b/gtsam/base/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(base "test*.cpp" "" "gtsam")
diff --git a/gtsam/discrete/CMakeLists.txt b/gtsam/discrete/CMakeLists.txt
index 3f1a57e0b..3c5602080 100644
--- a/gtsam/discrete/CMakeLists.txt
+++ b/gtsam/discrete/CMakeLists.txt
@@ -4,13 +4,8 @@ file(GLOB discrete_headers "*.h")
 # FIXME: exclude headers
 install(FILES ${discrete_headers} DESTINATION include/gtsam/discrete)
 
-# Exclude tests that don't work
-set (discrete_excluded_tests "")
-
 # Add all tests
-if (GTSAM_BUILD_TESTS)
-    gtsam_add_subdir_tests(discrete "gtsam" "gtsam" "${discrete_excluded_tests}")
-endif()
+add_subdirectory(tests)
 
 # Build timing scripts
 #if (GTSAM_BUILD_TIMING)
diff --git a/gtsam/discrete/tests/CMakeLists.txt b/gtsam/discrete/tests/CMakeLists.txt
new file mode 100644
index 000000000..e968fac91
--- /dev/null
+++ b/gtsam/discrete/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(discrete "test*.cpp" "" "gtsam")
diff --git a/gtsam/geometry/CMakeLists.txt b/gtsam/geometry/CMakeLists.txt
index 96b84193b..f72f965ea 100644
--- a/gtsam/geometry/CMakeLists.txt
+++ b/gtsam/geometry/CMakeLists.txt
@@ -2,16 +2,8 @@
 file(GLOB geometry_headers "*.h")
 install(FILES ${geometry_headers} DESTINATION include/gtsam/geometry)
 
-# Files to exclude from compilation of tests and timing scripts
-set(geometry_excluded_files
-# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test     
-    ""  # Add to this list, with full path, to exclude
-)
-
 # Build tests
-if (GTSAM_BUILD_TESTS)
-    gtsam_add_subdir_tests(geometry "gtsam" "gtsam" "${geometry_excluded_files}") 
-endif(GTSAM_BUILD_TESTS)
+add_subdirectory(tests)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 72124d25d..3cb41e936 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -45,10 +45,10 @@ Rot3 Rot3::rodriguez(const Sphere2& w, double theta) {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::Random(boost::random::mt19937 & rng) {
+Rot3 Rot3::Random(boost::mt19937 & rng) {
   // TODO allow any engine without including all of boost :-(
   Sphere2 w = Sphere2::Random(rng);
-  boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI);
+  boost::uniform_real<double> randomAngle(-M_PI,M_PI);
   double angle = randomAngle(rng);
   return rodriguez(w,angle);
 }
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 847d31d65..83232ea02 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -104,7 +104,7 @@ namespace gtsam {
     Rot3(const Quaternion& q);
 
     /// Random, generates a random axis, then random angle \in [-p,pi]
-    static Rot3 Random(boost::random::mt19937 & rng);
+    static Rot3 Random(boost::mt19937 & rng);
 
     /** Virtual destructor */
     virtual ~Rot3() {}
diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp
index ed2561e22..1dc64312a 100644
--- a/gtsam/geometry/Sphere2.cpp
+++ b/gtsam/geometry/Sphere2.cpp
@@ -22,6 +22,7 @@
 #include <gtsam/geometry/Point2.h>
 #include <boost/random/mersenne_twister.hpp>
 #include <boost/random/uniform_on_sphere.hpp>
+#include <boost/random/variate_generator.hpp>
 #include <iostream>
 
 using namespace std;
@@ -44,10 +45,14 @@ Sphere2 Sphere2::FromPoint3(const Point3& point,
 }
 
 /* ************************************************************************* */
-Sphere2 Sphere2::Random(boost::random::mt19937 & rng) {
+Sphere2 Sphere2::Random(boost::mt19937 & rng) {
   // TODO allow any engine without including all of boost :-(
-  boost::random::uniform_on_sphere<double> randomDirection(3);
-  vector<double> d = randomDirection(rng);
+  boost::uniform_on_sphere<double> randomDirection(3);
+  // This variate_generator object is required for versions of boost somewhere
+  // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
+  boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> >
+      generator(rng, randomDirection);
+  vector<double> d = generator();
   Sphere2 result;
   result.p_ = Point3(d[0], d[1], d[2]);
   return result;
diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h
index d4047f421..82c6bb3d7 100644
--- a/gtsam/geometry/Sphere2.h
+++ b/gtsam/geometry/Sphere2.h
@@ -22,23 +22,12 @@
 
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/base/DerivedValue.h>
+#include <boost/random/mersenne_twister.hpp>
 
 #ifndef SPHERE2_DEFAULT_COORDINATES_MODE
   #define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::RENORM
 #endif
 
-// (Cumbersome) forward declaration for random generator
-namespace boost {
-namespace random {
-template<class UIntType, std::size_t w, std::size_t n, std::size_t m,
-    std::size_t r, UIntType a, std::size_t u, UIntType d, std::size_t s,
-    UIntType b, std::size_t t, UIntType c, std::size_t l, UIntType f>
-class mersenne_twister_engine;
-typedef mersenne_twister_engine<uint32_t, 32, 624, 397, 31, 0x9908b0df, 11,
-    0xffffffff, 7, 0x9d2c5680, 15, 0xefc60000, 18, 1812433253> mt19937;
-}
-}
-
 namespace gtsam {
 
 /// Represents a 3D point on a unit sphere.
@@ -75,7 +64,7 @@ public:
       boost::optional<Matrix&> H = boost::none);
 
   /// Random direction, using boost::uniform_on_sphere
-  static Sphere2 Random(boost::random::mt19937 & rng);
+  static Sphere2 Random(boost::mt19937 & rng);
 
   /// @}
 
diff --git a/gtsam/geometry/tests/CMakeLists.txt b/gtsam/geometry/tests/CMakeLists.txt
new file mode 100644
index 000000000..b1499ee9d
--- /dev/null
+++ b/gtsam/geometry/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(geometry "test*.cpp" "" "gtsam")
diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp
index 306d84b94..15bdec407 100644
--- a/gtsam/geometry/tests/testSphere2.cpp
+++ b/gtsam/geometry/tests/testSphere2.cpp
@@ -312,7 +312,7 @@ TEST(Sphere2, localCoordinates_retract_expmap) {
 
 //*******************************************************************************
 TEST(Sphere2, Random) {
-  boost::random::mt19937 rng(42);
+  boost::mt19937 rng(42);
   // Check that is deterministic given same random seed
   Point3 expected(-0.667578, 0.671447, 0.321713);
   Point3 actual = Sphere2::Random(rng).point3();
diff --git a/gtsam/inference/CMakeLists.txt b/gtsam/inference/CMakeLists.txt
index d5c994b00..d5c37d976 100644
--- a/gtsam/inference/CMakeLists.txt
+++ b/gtsam/inference/CMakeLists.txt
@@ -2,16 +2,8 @@
 file(GLOB inference_headers "*.h")
 install(FILES ${inference_headers} DESTINATION include/gtsam/inference)
 
-# Files to exclude from compilation of tests and timing scripts
-set(inference_excluded_files
-# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test     
-    ""  # Add to this list, with full path, to exclude
-)
-
 # Build tests
-if (GTSAM_BUILD_TESTS)
-    gtsam_add_subdir_tests(inference "gtsam" "gtsam" "${inference_excluded_files}") 
-endif(GTSAM_BUILD_TESTS)
+add_subdirectory(tests)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
diff --git a/gtsam/inference/tests/CMakeLists.txt b/gtsam/inference/tests/CMakeLists.txt
new file mode 100644
index 000000000..aaa01d775
--- /dev/null
+++ b/gtsam/inference/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(inference "test*.cpp" "" "gtsam")
diff --git a/gtsam/linear/CMakeLists.txt b/gtsam/linear/CMakeLists.txt
index 4a93e2875..29eb60b93 100644
--- a/gtsam/linear/CMakeLists.txt
+++ b/gtsam/linear/CMakeLists.txt
@@ -2,21 +2,8 @@
 file(GLOB linear_headers "*.h")
 install(FILES ${linear_headers} DESTINATION include/gtsam/linear)
 
-# Files to exclude from compilation of tests and timing scripts
-set(linear_excluded_files
-# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test
-#    ""  # Add to this list, with full path, to exclude
-)
-
 # Build tests
-if (GTSAM_BUILD_TESTS)
-    gtsam_add_subdir_tests(linear "gtsam" "gtsam" "${linear_excluded_files}") 
-endif(GTSAM_BUILD_TESTS)
-
-if(MSVC)
-	set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerializationLinear.cpp"
-		APPEND PROPERTY COMPILE_FLAGS "/bigobj")
-endif()
+add_subdirectory(tests)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
diff --git a/gtsam/linear/GaussianConditional-inl.h b/gtsam/linear/GaussianConditional-inl.h
index dd0307c48..45d7ecc3b 100644
--- a/gtsam/linear/GaussianConditional-inl.h
+++ b/gtsam/linear/GaussianConditional-inl.h
@@ -19,9 +19,6 @@
 
 #pragma once
 
-#include <boost/range/join.hpp>
-#include <boost/assign/list_of.hpp>
-
 namespace gtsam {
 
   /* ************************************************************************* */
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index d8344abe1..cfa698dca 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -42,7 +42,6 @@
 #include <boost/assign/list_of.hpp>
 #include <boost/range/adaptor/transformed.hpp>
 #include <boost/range/adaptor/map.hpp>
-#include <boost/range/join.hpp>
 #include <boost/range/algorithm/copy.hpp>
 
 #include <sstream>
@@ -179,7 +178,7 @@ DenseIndex _getSizeHF(const Vector& m) { return m.size(); }
 /* ************************************************************************* */
 HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
     const std::vector<Vector>& gs, double f) :
-                        GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne((DenseIndex)1)))
+                        GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true)
 {
   // Get the number of variables
   size_t variable_count = js.size();
diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h
index f391d4afd..b2946b1fc 100644
--- a/gtsam/linear/JacobianFactor-inl.h
+++ b/gtsam/linear/JacobianFactor-inl.h
@@ -21,7 +21,6 @@
 #include <gtsam/linear/linearExceptions.h>
 #include <boost/assign/list_of.hpp>
 #include <boost/range/adaptor/transformed.hpp>
-#include <boost/range/join.hpp>
 #include <boost/range/algorithm/for_each.hpp>
 #include <boost/foreach.hpp>
 
@@ -123,7 +122,7 @@ namespace gtsam {
     // matrices, then extract the number of columns e.g. dimensions in each matrix.  Then joins with
     // a single '1' to add a dimension for the b vector.
     {
-      Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&internal::getColsJF), ListOfOne((DenseIndex)1)), b.size());
+      Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(), true);
     }
 
     // Check and add terms
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index e18055edc..1468810e6 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -283,7 +283,7 @@ namespace gtsam {
 
     // Allocate matrix and copy keys in order
     gttic(allocate);
-    Ab_ = VerticalBlockMatrix(boost::join(varDims, ListOfOne((DenseIndex)1)), m); // Allocate augmented matrix
+    Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix
     Base::keys_.resize(orderedSlots.size());
     boost::range::copy(    // Get variable keys
       orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin());
diff --git a/gtsam/linear/tests/CMakeLists.txt b/gtsam/linear/tests/CMakeLists.txt
new file mode 100644
index 000000000..d1aafb4ea
--- /dev/null
+++ b/gtsam/linear/tests/CMakeLists.txt
@@ -0,0 +1,6 @@
+gtsamAddTestsGlob(linear "test*.cpp" "" "gtsam")
+
+if(MSVC)
+	set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationLinear.cpp"
+		APPEND PROPERTY COMPILE_FLAGS "/bigobj")
+endif()
diff --git a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp
index ec7c01f88..8f37cac0c 100644
--- a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp
+++ b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp
@@ -114,39 +114,39 @@ TEST(GaussianBayesTree, complicatedMarginal) {
   // Create the conditionals to go in the BayesTree
   GaussianBayesTree bt;
   bt.insertRoot(
-    MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (11, (Matrix(3,1) << 0.0971, 0, 0).finished())
-                                                         (12, (Matrix(3,2) << 0.3171, 0.4387,  0.9502, 0.3816,  0, 0.7655).finished()),
-                                            2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished()), list_of
-      (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (9, (Matrix(3,1) << 0.7952, 0, 0).finished())
-                                                            (10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797).finished())
-                                                            (11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190).finished())
-                                                            (12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513).finished()),
-                                               2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished())))
-      (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (7, (Matrix(3,1) << 0.2551, 0, 0).finished())
-                                                            (8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575).finished())
-                                                            (11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143).finished()),
-                                               2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished()), list_of
-          (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (5, (Matrix(3,1) << 0.2435, 0, 0).finished())
-                                                                (6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308,    0.0).finished())
+    MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (11, (Matrix(3,1) << 0.0971, 0, 0)) 
+                                                         (12, (Matrix(3,2) << 0.3171, 0.4387,  0.9502, 0.3816,  0, 0.7655)),
+                                            2, (Vector(3) << 0.2638, 0.1455, 0.1361)), list_of
+      (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (9, (Matrix(3,1) << 0.7952, 0, 0))
+                                                            (10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797))
+                                                            (11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190))
+                                                            (12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513)),
+                                               2, (Vector(3) << 0.4314, 0.9106, 0.1818))))
+      (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (7, (Matrix(3,1) << 0.2551, 0, 0))
+                                                            (8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575))
+                                                            (11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143)),
+                                               2, (Vector(3) << 0.3998, 0.2599, 0.8001)), list_of
+          (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (5, (Matrix(3,1) << 0.2435, 0, 0))
+                                                                (6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308,    0.0))
                                                                 // NOTE the non-upper-triangular form
                                                                 // here since this test was written when we had column permutations
                                                                 // from LDL.  The code still works currently (does not enfore
                                                                 // upper-triangularity in this case) but this test will need to be
                                                                 // redone if this stops working in the future
-                                                                (7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172).finished())
-                                                                (8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759).finished()),
-                                                  2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished()), list_of
-              (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (3, (Matrix(3,1) << 0.0540, 0, 0).finished())
-                                                                    (4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371).finished())
-                                                                    (6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020).finished()),
-                                                      2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished())))
-              (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (1, (Matrix(3,1) << 0.2630, 0, 0).finished())
-                                                                    (2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524).finished())
-                                                                    (5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961).finished()),
-                                                      2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished())))))))));
+                                                                (7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172))
+                                                                (8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759)),
+                                                  2, (Vector(3) << 0.8173, 0.8687, 0.0844)), list_of
+              (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (3, (Matrix(3,1) << 0.0540, 0, 0))
+                                                                    (4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371))
+                                                                    (6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020)),
+                                                      2, (Vector(3) << 0.9619, 0.0046, 0.7749))))
+              (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (1, (Matrix(3,1) << 0.2630, 0, 0))
+                                                                    (2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524))
+                                                                    (5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961)),
+                                                      2, (Vector(3) << 0.0782, 0.4427, 0.1067))))))))));
 
   // Marginal on 5
-  Matrix expectedCov = (Matrix(1,1) << 236.5166).finished();
+  Matrix expectedCov = (Matrix(1,1) << 236.5166);
   //GaussianConditional actualJacobianChol = *bt.marginalFactor(5, EliminateCholesky);
   GaussianConditional actualJacobianQR = *bt.marginalFactor(5, EliminateQR);
   //EXPECT(assert_equal(actualJacobianChol, actualJacobianQR)); // Check that Chol and QR obtained marginals are the same
@@ -160,10 +160,10 @@ TEST(GaussianBayesTree, complicatedMarginal) {
   // Marginal on 6
 //  expectedCov = (Matrix(2,2) <<
 //      8471.2, 2886.2,
-//      2886.2, 1015.8).finished();
+//      2886.2, 1015.8);
   expectedCov = (Matrix(2,2) <<
       1015.8,    2886.2,
-      2886.2,    8471.2).finished();
+      2886.2,    8471.2);
   //actualJacobianChol = bt.marginalFactor(6, EliminateCholesky);
   actualJacobianQR = *bt.marginalFactor(6, EliminateQR);
   //EXPECT(assert_equal(actualJacobianChol, actualJacobianQR)); // Check that Chol and QR obtained marginals are the same
diff --git a/gtsam/navigation/CMakeLists.txt b/gtsam/navigation/CMakeLists.txt
index ed599cf15..3e82af774 100644
--- a/gtsam/navigation/CMakeLists.txt
+++ b/gtsam/navigation/CMakeLists.txt
@@ -2,13 +2,8 @@
 file(GLOB navigation_headers "*.h")
 install(FILES ${navigation_headers} DESTINATION include/gtsam/navigation)
 
-# Exclude tests that don't work
-set (navigation_excluded_tests "")
-
 # Add all tests
-if (GTSAM_BUILD_TESTS)
-	gtsam_add_subdir_tests(navigation "gtsam" "gtsam" "${navigation_excluded_tests}") 
-endif()
+add_subdirectory(tests)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
diff --git a/gtsam/navigation/tests/CMakeLists.txt b/gtsam/navigation/tests/CMakeLists.txt
new file mode 100644
index 000000000..4fd0b00d0
--- /dev/null
+++ b/gtsam/navigation/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(navigation "test*.cpp" "" "gtsam")
diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt
index f4cf0b585..b4d288104 100644
--- a/gtsam/nonlinear/CMakeLists.txt
+++ b/gtsam/nonlinear/CMakeLists.txt
@@ -2,16 +2,8 @@
 file(GLOB nonlinear_headers "*.h")
 install(FILES ${nonlinear_headers} DESTINATION include/gtsam/nonlinear)
 
-# Files to exclude from compilation of tests and timing scripts
-set(nonlinear_excluded_files
-# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test     
-    ""  # Add to this list, with full path, to exclude
-)
-
 # Build tests
-if (GTSAM_BUILD_TESTS)
-    gtsam_add_subdir_tests(nonlinear "gtsam" "gtsam" "${nonlinear_excluded_files}") 
-endif(GTSAM_BUILD_TESTS)
+add_subdirectory(tests)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
diff --git a/gtsam/nonlinear/tests/CMakeLists.txt b/gtsam/nonlinear/tests/CMakeLists.txt
new file mode 100644
index 000000000..69a3700f2
--- /dev/null
+++ b/gtsam/nonlinear/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam")
diff --git a/gtsam/slam/CMakeLists.txt b/gtsam/slam/CMakeLists.txt
index 1c20e3720..b348c4aa3 100644
--- a/gtsam/slam/CMakeLists.txt
+++ b/gtsam/slam/CMakeLists.txt
@@ -4,19 +4,10 @@ set (slam_excluded_headers #"")
 )
 
 file(GLOB slam_headers "*.h")
-list(REMOVE_ITEM slam_headers ${slam_excluded_headers})
 install(FILES ${slam_headers} DESTINATION include/gtsam/slam)
 
-# Files to exclude from compilation of tests and timing scripts
-set(slam_excluded_files
- "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp"      
-#    ""  # Add to this list, with full path, to exclude
-)
-
 # Build tests
-if (GTSAM_BUILD_TESTS)
-    gtsam_add_subdir_tests(slam "gtsam" "gtsam" "${slam_excluded_files}") 
-endif(GTSAM_BUILD_TESTS)
+add_subdirectory(tests)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h
index ada85667f..2d9b3fdd4 100644
--- a/gtsam/slam/PoseTranslationPrior.h
+++ b/gtsam/slam/PoseTranslationPrior.h
@@ -36,6 +36,9 @@ protected:
 
 public:
 
+  /** default constructor - only use for serialization */
+  PoseTranslationPrior() {}
+
   /** standard constructor */
   PoseTranslationPrior(Key key, const Translation& measured, const noiseModel::Base::shared_ptr& model)
   : Base(model, key), measured_(measured) {
diff --git a/gtsam/slam/tests/CMakeLists.txt b/gtsam/slam/tests/CMakeLists.txt
new file mode 100644
index 000000000..1205388a1
--- /dev/null
+++ b/gtsam/slam/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(slam "test*.cpp" "" "gtsam")
diff --git a/gtsam/symbolic/CMakeLists.txt b/gtsam/symbolic/CMakeLists.txt
index a683f0711..6535755f8 100644
--- a/gtsam/symbolic/CMakeLists.txt
+++ b/gtsam/symbolic/CMakeLists.txt
@@ -2,16 +2,8 @@
 file(GLOB symbolic_headers "*.h")
 install(FILES ${symbolic_headers} DESTINATION include/gtsam/symbolic)
 
-# Files to exclude from compilation of tests and timing scripts
-set(symbolic_excluded_files
-# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test     
-    ""  # Add to this list, with full path, to exclude
-)
-
 # Build tests
-if (GTSAM_BUILD_TESTS)
-    gtsam_add_subdir_tests(symbolic "gtsam" "gtsam" "${symbolic_excluded_files}") 
-endif(GTSAM_BUILD_TESTS)
+add_subdirectory(tests)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
diff --git a/gtsam/symbolic/tests/CMakeLists.txt b/gtsam/symbolic/tests/CMakeLists.txt
new file mode 100644
index 000000000..6bef7a109
--- /dev/null
+++ b/gtsam/symbolic/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(symbolic "test*.cpp" "" "gtsam")
diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index 4b7abdaba..6d4899050 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -105,7 +105,7 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
 	endif()
 
     # Wrap
-    wrap_and_install_library(gtsam_unstable.h "gtsam;gtsam_unstable" "" "${mexFlags}")
+    wrap_and_install_library(gtsam_unstable.h "gtsam" "" "${mexFlags}")
 endif(GTSAM_INSTALL_MATLAB_TOOLBOX)
 
 
diff --git a/gtsam_unstable/base/CMakeLists.txt b/gtsam_unstable/base/CMakeLists.txt
index 22737d533..d83e97d26 100644
--- a/gtsam_unstable/base/CMakeLists.txt
+++ b/gtsam_unstable/base/CMakeLists.txt
@@ -2,16 +2,8 @@
 file(GLOB base_headers "*.h")
 install(FILES ${base_headers} DESTINATION include/gtsam_unstable/base)
 
-set (base_full_libs
-    gtsam
-    gtsam_unstable)
-
-# Exclude tests that don't work
-set (base_excluded_tests "")
-
 # Add all tests
-gtsam_add_subdir_tests(base_unstable "${base_full_libs}" "${base_full_libs}" "${base_excluded_tests}") 
-add_dependencies(check.unstable check.base_unstable)
+add_subdirectory(tests)
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
diff --git a/gtsam_unstable/base/tests/CMakeLists.txt b/gtsam_unstable/base/tests/CMakeLists.txt
new file mode 100644
index 000000000..d103ec578
--- /dev/null
+++ b/gtsam_unstable/base/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(base_unstable "test*.cpp" "" "gtsam_unstable")
diff --git a/gtsam_unstable/discrete/CMakeLists.txt b/gtsam_unstable/discrete/CMakeLists.txt
index f6cfcb1a8..18346a45a 100644
--- a/gtsam_unstable/discrete/CMakeLists.txt
+++ b/gtsam_unstable/discrete/CMakeLists.txt
@@ -2,41 +2,8 @@
 file(GLOB discrete_headers "*.h")
 install(FILES ${discrete_headers} DESTINATION include/gtsam_unstable/discrete)
 
-set (discrete_full_libs
-    gtsam
-    gtsam_unstable)
-
-# Exclude tests that don't work
-#set (discrete_excluded_tests 
-#"${CMAKE_CURRENT_SOURCE_DIR}/tests/testScheduler.cpp"
-#)
-
-
 # Add all tests
-gtsam_add_subdir_tests(discrete_unstable "${discrete_full_libs}" "${discrete_full_libs}" "${discrete_excluded_tests}") 
-add_dependencies(check.unstable check.discrete_unstable)
+add_subdirectory(tests)
 
-# List examples to build - comment out here to exclude from compilation
-set(discrete_unstable_examples
-schedulingExample
-schedulingQuals12
-schedulingQuals13
-)
-
-if (GTSAM_BUILD_EXAMPLES)
-    foreach(example ${discrete_unstable_examples})
-    	add_executable(${example} "examples/${example}.cpp")
-    	
-    	# Disable building during make all/install
-        if (GTSAM_ENABLE_INSTALL_EXAMPLE_FIX)
-            set_target_properties(${example} PROPERTIES EXCLUDE_FROM_ALL ON)
-        endif()
-    	
-		if(NOT MSVC AND NOT XCODE_VERSION)
-			add_dependencies(examples ${example})
-			add_custom_target(${example}.run ${EXECUTABLE_OUTPUT_PATH}${example} ${ARGN})
-		endif()
-    	
-    	target_link_libraries(${example} gtsam gtsam_unstable)
-    endforeach(example)
-endif (GTSAM_BUILD_EXAMPLES)
+# Add examples
+add_subdirectory(examples)
diff --git a/gtsam_unstable/discrete/examples/CMakeLists.txt b/gtsam_unstable/discrete/examples/CMakeLists.txt
new file mode 100644
index 000000000..da06b7dfc
--- /dev/null
+++ b/gtsam_unstable/discrete/examples/CMakeLists.txt
@@ -0,0 +1,5 @@
+set (excluded_examples
+    # fileToExclude.cpp
+)
+
+gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable")
diff --git a/gtsam_unstable/discrete/tests/CMakeLists.txt b/gtsam_unstable/discrete/tests/CMakeLists.txt
new file mode 100644
index 000000000..2687a760c
--- /dev/null
+++ b/gtsam_unstable/discrete/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(discrete_unstable "test*.cpp" "" "gtsam_unstable")
diff --git a/gtsam_unstable/dynamics/CMakeLists.txt b/gtsam_unstable/dynamics/CMakeLists.txt
index 382b2b4cf..66aef9455 100644
--- a/gtsam_unstable/dynamics/CMakeLists.txt
+++ b/gtsam_unstable/dynamics/CMakeLists.txt
@@ -2,14 +2,5 @@
 file(GLOB dynamics_headers "*.h")
 install(FILES ${dynamics_headers} DESTINATION include/gtsam_unstable/dynamics)
 
-# Components to link tests in this subfolder against
-set (dynamics_full_libs
-    gtsam
-    gtsam_unstable)
-
-# Exclude tests that don't work
-set (dynamics_excluded_tests "")
-
 # Add all tests
-gtsam_add_subdir_tests(dynamics_unstable "${dynamics_full_libs}" "${dynamics_full_libs}" "${dynamics_excluded_tests}") 
-add_dependencies(check.unstable check.dynamics_unstable)
+add_subdirectory(tests)
diff --git a/gtsam_unstable/dynamics/tests/CMakeLists.txt b/gtsam_unstable/dynamics/tests/CMakeLists.txt
new file mode 100644
index 000000000..493cef4fa
--- /dev/null
+++ b/gtsam_unstable/dynamics/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(dynamics_unstable "test*.cpp" "" "gtsam_unstable")
diff --git a/gtsam_unstable/examples/CMakeLists.txt b/gtsam_unstable/examples/CMakeLists.txt
index d95ab7706..da06b7dfc 100644
--- a/gtsam_unstable/examples/CMakeLists.txt
+++ b/gtsam_unstable/examples/CMakeLists.txt
@@ -1,32 +1,5 @@
-if(NOT MSVC)
-  add_custom_target(unstable_examples)
-endif()
-
-# Build example executables
-FILE(GLOB example_srcs "*.cpp")
-foreach(example_src ${example_srcs} )
-    get_filename_component(example_base ${example_src} NAME_WE)
-    set( example_bin ${example_base} )
-    message(STATUS "Adding Example ${example_bin}")
-	if(NOT MSVC)
-      add_dependencies(examples ${example_bin})
-	endif()
-    add_executable(${example_bin} ${example_src})
-    
-    # Disable building during make all/install
-    if (GTSAM_DISABLE_EXAMPLES_ON_INSTALL)
-        set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
-    endif()
-    
-    target_link_libraries(${example_bin} gtsam gtsam_unstable)
-	if(NOT MSVC AND NOT XCODE_VERSION)
-      add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN})
-	endif()
-	
-	# Set up Visual Studio folder
-	if(MSVC)
-	  set_property(TARGET ${example_bin} PROPERTY FOLDER "Examples")
-	endif()
-
-endforeach(example_src)
+set (excluded_examples
+    # fileToExclude.cpp
+)
 
+gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable")
diff --git a/gtsam_unstable/geometry/CMakeLists.txt b/gtsam_unstable/geometry/CMakeLists.txt
index c7ee3902f..c8b7e250f 100644
--- a/gtsam_unstable/geometry/CMakeLists.txt
+++ b/gtsam_unstable/geometry/CMakeLists.txt
@@ -2,14 +2,5 @@
 file(GLOB geometry_headers "*.h")
 install(FILES ${geometry_headers} DESTINATION include/gtsam_unstable/geometry)
 
-# Components to link tests in this subfolder against
-set (geometry_full_libs
-    gtsam
-    gtsam_unstable)
-
-# Exclude tests that don't work
-set (geometry_excluded_tests "")
-
 # Add all tests
-gtsam_add_subdir_tests(geometry_unstable "${geometry_full_libs}" "${geometry_full_libs}" "${geometry_excluded_tests}") 
-add_dependencies(check.unstable check.geometry_unstable)
+add_subdirectory(tests)
diff --git a/gtsam_unstable/geometry/tests/CMakeLists.txt b/gtsam_unstable/geometry/tests/CMakeLists.txt
new file mode 100644
index 000000000..e22da8652
--- /dev/null
+++ b/gtsam_unstable/geometry/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(geometry_unstable "test*.cpp" "" "gtsam_unstable")
diff --git a/gtsam_unstable/nonlinear/CMakeLists.txt b/gtsam_unstable/nonlinear/CMakeLists.txt
index daef8983c..9e0cb68e1 100644
--- a/gtsam_unstable/nonlinear/CMakeLists.txt
+++ b/gtsam_unstable/nonlinear/CMakeLists.txt
@@ -2,18 +2,5 @@
 file(GLOB nonlinear_headers "*.h")
 install(FILES ${nonlinear_headers} DESTINATION include/gtsam_unstable/nonlinear)
 
-# Components to link tests in this subfolder against
-set (nonlinear_full_libs
-gtsam
-gtsam_unstable)
-
-# Exclude tests that don't work
-set (nonlinear_excluded_tests #"")
-#"${CMAKE_CURRENT_SOURCE_DIR}/tests/testConcurrentIncrementalFilter.cpp" 
-#"${CMAKE_CURRENT_SOURCE_DIR}/tests/testIncrementalFixedLagSmoother.cpp"
-)
-
-
 # Add all tests
-gtsam_add_subdir_tests(nonlinear_unstable "${nonlinear_full_libs}" "${nonlinear_full_libs}" "${nonlinear_excluded_tests}")
-add_dependencies(check.unstable check.nonlinear_unstable)
+add_subdirectory(tests)
diff --git a/gtsam_unstable/nonlinear/tests/CMakeLists.txt b/gtsam_unstable/nonlinear/tests/CMakeLists.txt
new file mode 100644
index 000000000..5b1fd07d4
--- /dev/null
+++ b/gtsam_unstable/nonlinear/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "" "gtsam_unstable")
diff --git a/gtsam_unstable/slam/CMakeLists.txt b/gtsam_unstable/slam/CMakeLists.txt
index ae4784f70..a86beac63 100644
--- a/gtsam_unstable/slam/CMakeLists.txt
+++ b/gtsam_unstable/slam/CMakeLists.txt
@@ -7,16 +7,5 @@ file(GLOB slam_headers "*.h")
 list(REMOVE_ITEM slam_headers ${slam_excluded_headers})
 install(FILES ${slam_headers} DESTINATION include/gtsam_unstable/slam)
 
-# Components to link tests in this subfolder against
-set (slam_full_libs
-    gtsam
-    gtsam_unstable)
-
-# Exclude tests that don't work
-set (slam_excluded_tests
-    "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp"
-#    ""  # Add to this list, with full path, to exclude
-)
 # Add all tests
-gtsam_add_subdir_tests(slam_unstable "${slam_full_libs}" "${slam_full_libs}" "${slam_excluded_tests}") 
-add_dependencies(check.unstable check.slam_unstable)
+add_subdirectory(tests)
diff --git a/gtsam_unstable/slam/tests/CMakeLists.txt b/gtsam_unstable/slam/tests/CMakeLists.txt
new file mode 100644
index 000000000..bb5259ef2
--- /dev/null
+++ b/gtsam_unstable/slam/tests/CMakeLists.txt
@@ -0,0 +1,7 @@
+
+# Exclude tests that don't work
+set (slam_excluded_tests
+    testSerialization.cpp
+)
+
+gtsamAddTestsGlob(slam_unstable "test*.cpp" "${slam_excluded_tests}" "gtsam_unstable")
diff --git a/matlab/+gtsam/covarianceEllipse.m b/matlab/+gtsam/covarianceEllipse.m
index 387d7cd12..cdc239bb2 100644
--- a/matlab/+gtsam/covarianceEllipse.m
+++ b/matlab/+gtsam/covarianceEllipse.m
@@ -1,4 +1,4 @@
-function covarianceEllipse(x,P,color, k)
+function h = covarianceEllipse(x,P,color, k)
 % covarianceEllipse plots a Gaussian as an uncertainty ellipse
 % Based on Maybeck Vol 1, page 366
 % k=2.296 corresponds to 1 std, 68.26% of all probability
@@ -13,7 +13,7 @@ s1 = s(1,1);
 s2 = s(2,2);
 if nargin<4, k = 2.296; end;
 [ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) );
-line(ex,ey,'color',color);
+h = line(ex,ey,'color',color);
 
     function [x,y] = ellipse(a,b,c);
         % ellipse: return the x and y coordinates for an ellipse
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index a6ae2afca..cf81dc762 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -1,31 +1,19 @@
-# Assemble local libraries
-set (tests_full_libs
-    gtsam
-    CppUnitLite)
-
 # exclude certain files
 # note the source dir on each 
-set (tests_exclude
-    #"${CMAKE_CURRENT_SOURCE_DIR}/testOccupancyGrid.cpp"
-)
+set (tests_exclude "")
 
 if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") # might not be best test - Richard & Jason & Frank
 	# clang linker segfaults on large testSerializationSLAM 
-	list (APPEND tests_exclude "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp") 
+	list (APPEND tests_exclude "testSerializationSLAM.cpp") 
 endif()
 
 # Build tests
-if (GTSAM_BUILD_TESTS)
-    # Subdirectory target for tests
-    add_custom_target(check.tests COMMAND ${CMAKE_CTEST_COMMAND})
-    set(is_test TRUE)
+gtsamAddTestsGlob(tests "test*.cpp" "${tests_exclude}" "gtsam")
 
-    # Build grouped tests
-    gtsam_add_grouped_scripts("tests"               # Use subdirectory as group label
-    "test*.cpp;*.h" check "Test"                      # Standard for all tests
-    "${tests_full_libs}" "${tests_full_libs}" "${tests_exclude}"  # Pass in linking and exclusion lists
-    ${is_test})                                         # Set all as tests
-endif (GTSAM_BUILD_TESTS)
+if(MSVC)
+	set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp"
+		APPEND PROPERTY COMPILE_FLAGS "/bigobj")
+endif()
 
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
@@ -39,8 +27,3 @@ if (GTSAM_BUILD_TIMING)
     "${tests_full_libs}" "${tests_full_libs}" "${tests_exclude}"   # Pass in linking and exclusion lists
     ${is_test})
 endif (GTSAM_BUILD_TIMING)
-
-if(MSVC)
-	set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp"
-		APPEND PROPERTY COMPILE_FLAGS "/bigobj")
-endif()
diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt
index 28f8bea48..03915a662 100644
--- a/wrap/CMakeLists.txt
+++ b/wrap/CMakeLists.txt
@@ -31,9 +31,5 @@ set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 install(FILES matlab.h DESTINATION include/wrap)
 
 # Build tests
-if (GTSAM_BUILD_TESTS)
-    set(wrap_local_libs wrap_lib ${WRAP_BOOST_LIBRARIES}) 
-    gtsam_add_subdir_tests("wrap" "${wrap_local_libs}" "${wrap_local_libs}" "") 
-endif(GTSAM_BUILD_TESTS)
-
+add_subdirectory(tests)
 
diff --git a/wrap/tests/CMakeLists.txt b/wrap/tests/CMakeLists.txt
new file mode 100644
index 000000000..f3ac1df49
--- /dev/null
+++ b/wrap/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(wrap "test*.cpp" "" "wrap_lib")