diff --git a/Makefile.am b/Makefile.am index d50973faf..0d9b8c8f6 100644 --- a/Makefile.am +++ b/Makefile.am @@ -11,7 +11,7 @@ ACLOCAL_AMFLAGS = -I m4 AUTOMAKE_OPTIONS = foreign nostdinc # All the sub-directories that need to be built -SUBDIRS = CppUnitLite base geometry inference linear nonlinear slam . tests wrap examples +SUBDIRS = CppUnitLite base geometry inference linear nonlinear slam . tests wrap examples # And the corresponding libraries produced SUBLIBS = base/libbase.la geometry/libgeometry.la inference/libinference.la \ diff --git a/configure.ac b/configure.ac index e6769389c..b4e46d56a 100644 --- a/configure.ac +++ b/configure.ac @@ -4,7 +4,7 @@ AC_PREREQ(2.59) AC_INIT(gtsam, 0.0.0, dellaert@cc.gatech.edu) AM_INIT_AUTOMAKE(gtsam, 0.0.0) -AC_OUTPUT(Makefile CppUnitLite/Makefile base/Makefile inference/Makefile linear/Makefile geometry/Makefile nonlinear/Makefile slam/Makefile tests/Makefile wrap/Makefile examples/Makefile) +AC_OUTPUT(Makefile CppUnitLite/Makefile base/Makefile inference/Makefile linear/Makefile geometry/Makefile nonlinear/Makefile slam/Makefile tests/Makefile wrap/Makefile examples/Makefile examples/vSLAMexample/Makefile) AC_CONFIG_MACRO_DIR([m4]) AC_CONFIG_HEADER([config.h]) AC_CONFIG_SRCDIR([CppUnitLite/Test.cpp]) diff --git a/examples/Makefile.am b/examples/Makefile.am index a0b0f4621..0f0570cdc 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -15,7 +15,7 @@ noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation v noinst_PROGRAMS += PlanarSLAMExample # Solves SLAM example from tutorial by using planarSLAM noinst_PROGRAMS += Pose2SLAMExample # Solves SLAM example from tutorial by using planarSLAM noinst_PROGRAMS += PlanarSLAMSelfContained # Solves SLAM example from tutorial with all typedefs in the script - +SUBDIRS = vSLAMexample #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/examples/vSLAMexample/Data/calib.txt b/examples/vSLAMexample/Data/calib.txt new file mode 100644 index 000000000..8eeb7e656 --- /dev/null +++ b/examples/vSLAMexample/Data/calib.txt @@ -0,0 +1 @@ +800 600 -1119.61507797 1119.61507797 399.5 299.5 diff --git a/examples/vSLAMexample/Data/landmarks.txt b/examples/vSLAMexample/Data/landmarks.txt new file mode 100644 index 000000000..f6dd9714e --- /dev/null +++ b/examples/vSLAMexample/Data/landmarks.txt @@ -0,0 +1,9 @@ +7 +0 0 0 0 +1 10 0 0 +2 0 10 0 +3 10 10 0 +4 0 0 10 +5 10 0 10 +6 0 10 10 + diff --git a/examples/vSLAMexample/Data/ttpy10.feat b/examples/vSLAMexample/Data/ttpy10.feat new file mode 100644 index 000000000..265c3dfde --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy10.feat @@ -0,0 +1,8 @@ +7 +6 424 190 +4 399.553 240.574 +2 422.974 248.632 +5 480.082 258.082 +3 496.146 264.634 +0 399.5 299.5 +1 475.915 317.106 diff --git a/examples/vSLAMexample/Data/ttpy10.pose b/examples/vSLAMexample/Data/ttpy10.pose new file mode 100644 index 000000000..ee0f6231a --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy10.pose @@ -0,0 +1,4 @@ +0.939693 0.34202 0 0 +-0.241845 0.664463 -0.707107 0 +-0.241845 0.664463 0.707107 0 +34.202 -93.9693 100 1 diff --git a/examples/vSLAMexample/Data/ttpy100.feat b/examples/vSLAMexample/Data/ttpy100.feat new file mode 100644 index 000000000..fb16b2777 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy100.feat @@ -0,0 +1,8 @@ +7 +5 321.545 223.591 +4 399.553 240.574 +1 325.395 282.512 +6 372.434 296.453 +0 399.5 299.5 +3 296.479 336.708 +2 373.796 355.347 diff --git a/examples/vSLAMexample/Data/ttpy100.pose b/examples/vSLAMexample/Data/ttpy100.pose new file mode 100644 index 000000000..4225e3d4e --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy100.pose @@ -0,0 +1,4 @@ +-0.939693 -0.34202 0 0 +0.241845 -0.664463 -0.707107 0 +0.241845 -0.664463 0.707107 0 +-34.202 93.9693 100 1 diff --git a/examples/vSLAMexample/Data/ttpy20.feat b/examples/vSLAMexample/Data/ttpy20.feat new file mode 100644 index 000000000..0afb761b9 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy20.feat @@ -0,0 +1,8 @@ +7 +6 448.854 198.317 +4 399.574 240.532 +2 446.39 257.049 +5 467.479 276.146 +3 509.674 289.86 +0 399.5 299.5 +1 463.827 335.115 diff --git a/examples/vSLAMexample/Data/ttpy20.pose b/examples/vSLAMexample/Data/ttpy20.pose new file mode 100644 index 000000000..f8b252855 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy20.pose @@ -0,0 +1,4 @@ +0.766044 0.642788 0 0 +-0.454519 0.541675 -0.707107 0 +-0.454519 0.541675 0.707107 0 +64.2788 -76.6044 100 1 diff --git a/examples/vSLAMexample/Data/ttpy30.feat b/examples/vSLAMexample/Data/ttpy30.feat new file mode 100644 index 000000000..412417476 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy30.feat @@ -0,0 +1,8 @@ +7 +6 468.239 211.826 +4 399.319 240.617 +2 464.725 270.725 +5 445.7 290.2 +0 399.5 299.5 +3 510.239 317.674 +1 443.471 349.078 diff --git a/examples/vSLAMexample/Data/ttpy30.pose b/examples/vSLAMexample/Data/ttpy30.pose new file mode 100644 index 000000000..f7ad9f674 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy30.pose @@ -0,0 +1,4 @@ +0.5 0.866025 0 0 +-0.612372 0.353553 -0.707107 0 +-0.612372 0.353553 0.707107 0 +86.6025 -50 100 1 diff --git a/examples/vSLAMexample/Data/ttpy40.feat b/examples/vSLAMexample/Data/ttpy40.feat new file mode 100644 index 000000000..0e298272b --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy40.feat @@ -0,0 +1,8 @@ +7 +6 480.111 229.111 +4 399.556 240.556 +2 476.163 287.93 +5 417.685 298.056 +0 399.605 299.535 +3 497.1 344.3 +1 416.846 357.038 diff --git a/examples/vSLAMexample/Data/ttpy40.pose b/examples/vSLAMexample/Data/ttpy40.pose new file mode 100644 index 000000000..e9c9cd251 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy40.pose @@ -0,0 +1,4 @@ +0.173648 0.984808 0 0 +-0.696364 0.122788 -0.707107 0 +-0.696364 0.122788 0.707107 0 +98.4808 -17.3648 100 1 diff --git a/examples/vSLAMexample/Data/ttpy50.feat b/examples/vSLAMexample/Data/ttpy50.feat new file mode 100644 index 000000000..5ec007888 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy50.feat @@ -0,0 +1,8 @@ +7 +4 399.429 240.551 +6 482.812 248.312 +5 387.364 298.964 +0 399.578 299.556 +2 478.63 307.391 +1 387.98 357.804 +3 470.922 366.471 diff --git a/examples/vSLAMexample/Data/ttpy50.pose b/examples/vSLAMexample/Data/ttpy50.pose new file mode 100644 index 000000000..9c84b54f3 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy50.pose @@ -0,0 +1,4 @@ +-0.173648 0.984808 0 0 +-0.696364 -0.122788 -0.707107 0 +-0.696364 -0.122788 0.707107 0 +98.4808 17.3648 100 1 diff --git a/examples/vSLAMexample/Data/ttpy60.feat b/examples/vSLAMexample/Data/ttpy60.feat new file mode 100644 index 000000000..05237171f --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy60.feat @@ -0,0 +1,8 @@ +7 +4 399.478 240.565 +6 474.941 267.451 +5 358.473 292.364 +0 399.5 299.5 +2 471.043 326.447 +1 360.68 351.22 +3 434.473 380.709 diff --git a/examples/vSLAMexample/Data/ttpy60.pose b/examples/vSLAMexample/Data/ttpy60.pose new file mode 100644 index 000000000..9c0a8c942 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy60.pose @@ -0,0 +1,4 @@ +-0.5 0.866025 0 0 +-0.612372 -0.353553 -0.707107 0 +-0.612372 -0.353553 0.707107 0 +86.6025 50 100 1 diff --git a/examples/vSLAMexample/Data/ttpy70.feat b/examples/vSLAMexample/Data/ttpy70.feat new file mode 100644 index 000000000..fd5af8c99 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy70.feat @@ -0,0 +1,8 @@ +7 +4 399.553 240.617 +5 335.294 279.275 +6 457.717 283.698 +0 399.5 299.5 +1 338.531 338.265 +2 454.54 342.64 +3 393.218 384.691 diff --git a/examples/vSLAMexample/Data/ttpy70.pose b/examples/vSLAMexample/Data/ttpy70.pose new file mode 100644 index 000000000..0c2254de6 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy70.pose @@ -0,0 +1,4 @@ +-0.766044 0.642788 0 0 +-0.454519 -0.541675 -0.707107 0 +-0.454519 -0.541675 0.707107 0 +64.2788 76.6044 100 1 diff --git a/examples/vSLAMexample/Data/ttpy80.feat b/examples/vSLAMexample/Data/ttpy80.feat new file mode 100644 index 000000000..465f80804 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy80.feat @@ -0,0 +1,8 @@ +7 +4 399.5 240.5 +5 320.667 261.958 +6 432.389 295.111 +0 399.5 299.5 +1 324.653 320.898 +2 430.5 353.96 +3 352.737 377.474 diff --git a/examples/vSLAMexample/Data/ttpy80.pose b/examples/vSLAMexample/Data/ttpy80.pose new file mode 100644 index 000000000..9d6491304 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy80.pose @@ -0,0 +1,4 @@ +-0.939693 0.34202 0 0 +-0.241845 -0.664463 -0.707107 0 +-0.241845 -0.664463 0.707107 0 +34.202 93.9693 100 1 diff --git a/examples/vSLAMexample/Data/ttpy90.feat b/examples/vSLAMexample/Data/ttpy90.feat new file mode 100644 index 000000000..6731831c0 --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy90.feat @@ -0,0 +1,8 @@ +7 +4 399.5 240.5 +5 316 242.5 +0 397 299.5 +6 402.765 299.588 +1 320.5 301.5 +2 402.5 358.5 +3 319 360.5 diff --git a/examples/vSLAMexample/Data/ttpy90.pose b/examples/vSLAMexample/Data/ttpy90.pose new file mode 100644 index 000000000..b51c1a58f --- /dev/null +++ b/examples/vSLAMexample/Data/ttpy90.pose @@ -0,0 +1,4 @@ +-1 0 0 0 +0 -0.707107 -0.707107 0 +0 -0.707107 0.707107 0 +0 100 100 1 diff --git a/examples/vSLAMexample/Feature2D.cpp b/examples/vSLAMexample/Feature2D.cpp new file mode 100644 index 000000000..30bd7fc48 --- /dev/null +++ b/examples/vSLAMexample/Feature2D.cpp @@ -0,0 +1,2 @@ +#include "Feature2D.h" + diff --git a/examples/vSLAMexample/Feature2D.h b/examples/vSLAMexample/Feature2D.h new file mode 100644 index 000000000..cbbc8665a --- /dev/null +++ b/examples/vSLAMexample/Feature2D.h @@ -0,0 +1,23 @@ +#ifndef FEATURE2D_H +#define FEATURE2D_H + +#include "gtsam/geometry/Point2.h" +#include + +class Feature2D +{ +public: + gtsam::Point2 m_p; + int m_id; // id of the 3D landmark that it is associated with +public: + Feature2D(int id, gtsam::Point2 p):m_id(id), m_p(p) {}; + + void print(const std::string& s = "") const + { + std::cout << s << std::endl; + std::cout << "Id: " << m_id << std::endl; + m_p.print(); + } +}; + +#endif // FEATURE2D_H diff --git a/examples/vSLAMexample/FeatureDetector.cpp b/examples/vSLAMexample/FeatureDetector.cpp new file mode 100644 index 000000000..9d93fbbb1 --- /dev/null +++ b/examples/vSLAMexample/FeatureDetector.cpp @@ -0,0 +1,88 @@ +#include "FeatureDetector.h" +#include "landmarkUtils.h" +#include +#include + +using namespace cv; +using namespace std; +using namespace gtsam; + +FeatureDetector::FeatureDetector(const char* colorDescFile) +{ + readLandMarkColors(colorDescFile, vColors_); +} + +int FeatureDetector::findColorId(const RGBColor& color) +{ + for (size_t i = 0; i FeatureDetector::detect(const cv::Mat& image) +{ + vFeatures_.clear(); + + RGBColor BLACK(0,0,0); + map numPixels; // + for (int y = 0; y(y,x) == BLACK) + continue; + + RGBColor color = image.at(y,x); + int t = color[2]; color[2] = color[0]; color[0] = t; + int colorId = findColorId(color); + + if (colorId == -1) + continue; + + map::iterator it = numPixels.find(colorId) ; + if (it == numPixels.end()) + { + numPixels[colorId] = 1; + vFeatures_.push_back(Feature2D(colorId, Point2(x,y))); + } + else + { + size_t k = 0; + for (; k& FeatureDetector::read(const char* filename) +{ + ifstream file(filename); + int numFeatures; + file >> numFeatures ; + + vFeatures_.clear(); + for (size_t i = 0; i < numFeatures; i++) + { + int id; double x, y; + file >> id >> x >> y; + vFeatures_.push_back(Feature2D(id, Point2(x, y))); + } + + file.close(); + return vFeatures_; +} diff --git a/examples/vSLAMexample/FeatureDetector.h b/examples/vSLAMexample/FeatureDetector.h new file mode 100644 index 000000000..59968ccd1 --- /dev/null +++ b/examples/vSLAMexample/FeatureDetector.h @@ -0,0 +1,23 @@ +#ifndef FEATUREDETECTOR_H +#define FEATUREDETECTOR_H + +#include "Feature2D.h" +#include +#include "cv.h" +#include "landmarkUtils.h" + + +/** + * A simple Feature Detector to detect color-coded features in the image. + * A list of colors must be provided. Feature id is the id of its associated color. + */ +class FeatureDetector +{ +private: + std::vector vFeatures_; + +public: + std::vector& read(const char* filename); +}; + +#endif // FEATUREDETECTOR_H diff --git a/examples/vSLAMexample/Makefile.am b/examples/vSLAMexample/Makefile.am new file mode 100644 index 000000000..fa254de8b --- /dev/null +++ b/examples/vSLAMexample/Makefile.am @@ -0,0 +1,32 @@ +#---------------------------------------------------------------------------------------------------- +# GTSAM Examples +#---------------------------------------------------------------------------------------------------- + +# use nostdinc to turn off -I. and -I.., we do not need them because +# header files are qualified so they can be included in external projects. +AUTOMAKE_OPTIONS = nostdinc + +headers = +sources = +check_PROGRAMS = + +# Examples +noinst_PROGRAMS = vSLAMexample +vSLAMexample_includedir = . +vSLAMexample_SOURCES = vSLAMexample.cpp Feature2D.cpp landmarkUtils.cpp +#---------------------------------------------------------------------------------------------------- +# rules to build local programs +#---------------------------------------------------------------------------------------------------- +AM_LDFLAGS = $(BOOST_LDFLAGS) +AM_CPPFLAGS = -I$(boost) -I$(SparseInc) -I$(top_srcdir)/.. +LDADD = ../../libgtsam.la +AM_DEFAULT_SOURCE_EXT = .cpp + +# rule to run an executable +%.run: % $(LDADD) + ./$^ + +# rule to run executable with valgrind +%.valgrind: % $(LDADD) + valgrind ./$^ +#---------------------------------------------------------------------------------------------------- diff --git a/examples/vSLAMexample/Makefile.in b/examples/vSLAMexample/Makefile.in new file mode 100644 index 000000000..91c72a3e6 --- /dev/null +++ b/examples/vSLAMexample/Makefile.in @@ -0,0 +1,520 @@ +# Makefile.in generated by automake 1.11.1 from Makefile.am. +# @configure_input@ + +# Copyright (C) 1994, 1995, 1996, 1997, 1998, 1999, 2000, 2001, 2002, +# 2003, 2004, 2005, 2006, 2007, 2008, 2009 Free Software Foundation, +# Inc. +# This Makefile.in is free software; the Free Software Foundation +# gives unlimited permission to copy and/or distribute it, +# with or without modifications, as long as this notice is preserved. + +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY, to the extent permitted by law; without +# even the implied warranty of MERCHANTABILITY or FITNESS FOR A +# PARTICULAR PURPOSE. + +@SET_MAKE@ + +#---------------------------------------------------------------------------------------------------- +# GTSAM Examples +#---------------------------------------------------------------------------------------------------- + +VPATH = @srcdir@ +pkgdatadir = $(datadir)/@PACKAGE@ +pkgincludedir = $(includedir)/@PACKAGE@ +pkglibdir = $(libdir)/@PACKAGE@ +pkglibexecdir = $(libexecdir)/@PACKAGE@ +am__cd = CDPATH="$${ZSH_VERSION+.}$(PATH_SEPARATOR)" && cd +install_sh_DATA = $(install_sh) -c -m 644 +install_sh_PROGRAM = $(install_sh) -c +install_sh_SCRIPT = $(install_sh) -c +INSTALL_HEADER = $(INSTALL_DATA) +transform = $(program_transform_name) +NORMAL_INSTALL = : +PRE_INSTALL = : +POST_INSTALL = : +NORMAL_UNINSTALL = : +PRE_UNINSTALL = : +POST_UNINSTALL = : +build_triplet = @build@ +host_triplet = @host@ +check_PROGRAMS = +noinst_PROGRAMS = vSLAMexample$(EXEEXT) +subdir = examples/vSLAMexample +DIST_COMMON = $(srcdir)/Makefile.am $(srcdir)/Makefile.in +ACLOCAL_M4 = $(top_srcdir)/aclocal.m4 +am__aclocal_m4_deps = $(top_srcdir)/m4/libtool.m4 \ + $(top_srcdir)/m4/ltoptions.m4 $(top_srcdir)/m4/ltsugar.m4 \ + $(top_srcdir)/m4/ltversion.m4 $(top_srcdir)/m4/lt~obsolete.m4 \ + $(top_srcdir)/configure.ac +am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \ + $(ACLOCAL_M4) +mkinstalldirs = $(install_sh) -d +CONFIG_HEADER = $(top_builddir)/config.h +CONFIG_CLEAN_FILES = +CONFIG_CLEAN_VPATH_FILES = +PROGRAMS = $(noinst_PROGRAMS) +am_vSLAMexample_OBJECTS = vSLAMexample.$(OBJEXT) Feature2D.$(OBJEXT) \ + landmarkUtils.$(OBJEXT) +vSLAMexample_OBJECTS = $(am_vSLAMexample_OBJECTS) +vSLAMexample_LDADD = $(LDADD) +vSLAMexample_DEPENDENCIES = ../../libgtsam.la +DEFAULT_INCLUDES = +depcomp = $(SHELL) $(top_srcdir)/depcomp +am__depfiles_maybe = depfiles +am__mv = mv -f +CXXCOMPILE = $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) \ + $(AM_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) +LTCXXCOMPILE = $(LIBTOOL) --tag=CXX $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) \ + --mode=compile $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) \ + $(AM_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) +CXXLD = $(CXX) +CXXLINK = $(LIBTOOL) --tag=CXX $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) \ + --mode=link $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) $(AM_LDFLAGS) \ + $(LDFLAGS) -o $@ +SOURCES = $(vSLAMexample_SOURCES) +DIST_SOURCES = $(vSLAMexample_SOURCES) +ETAGS = etags +CTAGS = ctags +DISTFILES = $(DIST_COMMON) $(DIST_SOURCES) $(TEXINFOS) $(EXTRA_DIST) +ACLOCAL = @ACLOCAL@ +AMTAR = @AMTAR@ +AR = @AR@ +AUTOCONF = @AUTOCONF@ +AUTOHEADER = @AUTOHEADER@ +AUTOMAKE = @AUTOMAKE@ +AWK = @AWK@ +CC = @CC@ +CCDEPMODE = @CCDEPMODE@ +CFLAGS = @CFLAGS@ +CPP = @CPP@ +CPPFLAGS = @CPPFLAGS@ +CXX = @CXX@ +CXXCPP = @CXXCPP@ +CXXDEPMODE = @CXXDEPMODE@ +CXXFLAGS = @CXXFLAGS@ +CYGPATH_W = @CYGPATH_W@ +DEFS = @DEFS@ +DEPDIR = @DEPDIR@ +DSYMUTIL = @DSYMUTIL@ +DUMPBIN = @DUMPBIN@ +ECHO_C = @ECHO_C@ +ECHO_N = @ECHO_N@ +ECHO_T = @ECHO_T@ +EGREP = @EGREP@ +EXEEXT = @EXEEXT@ +FGREP = @FGREP@ +GREP = @GREP@ +INSTALL = @INSTALL@ +INSTALL_DATA = @INSTALL_DATA@ +INSTALL_PROGRAM = @INSTALL_PROGRAM@ +INSTALL_SCRIPT = @INSTALL_SCRIPT@ +INSTALL_STRIP_PROGRAM = @INSTALL_STRIP_PROGRAM@ +LD = @LD@ +LDFLAGS = @LDFLAGS@ +LIBOBJS = @LIBOBJS@ +LIBS = @LIBS@ +LIBTOOL = @LIBTOOL@ +LIPO = @LIPO@ +LN_S = @LN_S@ +LTLIBOBJS = @LTLIBOBJS@ +MAKEINFO = @MAKEINFO@ +MKDIR_P = @MKDIR_P@ +NM = @NM@ +NMEDIT = @NMEDIT@ +OBJDUMP = @OBJDUMP@ +OBJEXT = @OBJEXT@ +OTOOL = @OTOOL@ +OTOOL64 = @OTOOL64@ +PACKAGE = @PACKAGE@ +PACKAGE_BUGREPORT = @PACKAGE_BUGREPORT@ +PACKAGE_NAME = @PACKAGE_NAME@ +PACKAGE_STRING = @PACKAGE_STRING@ +PACKAGE_TARNAME = @PACKAGE_TARNAME@ +PACKAGE_URL = @PACKAGE_URL@ +PACKAGE_VERSION = @PACKAGE_VERSION@ +PATH_SEPARATOR = @PATH_SEPARATOR@ +RANLIB = @RANLIB@ +SED = @SED@ +SET_MAKE = @SET_MAKE@ +SHELL = @SHELL@ +STRIP = @STRIP@ +SparseInc = @SparseInc@ +SparseLib = @SparseLib@ +VERSION = @VERSION@ +abs_builddir = @abs_builddir@ +abs_srcdir = @abs_srcdir@ +abs_top_builddir = @abs_top_builddir@ +abs_top_srcdir = @abs_top_srcdir@ +ac_ct_CC = @ac_ct_CC@ +ac_ct_CXX = @ac_ct_CXX@ +ac_ct_DUMPBIN = @ac_ct_DUMPBIN@ +am__include = @am__include@ +am__leading_dot = @am__leading_dot@ +am__quote = @am__quote@ +am__tar = @am__tar@ +am__untar = @am__untar@ +bindir = @bindir@ +boost = @boost@ +build = @build@ +build_alias = @build_alias@ +build_cpu = @build_cpu@ +build_os = @build_os@ +build_vendor = @build_vendor@ +builddir = @builddir@ +datadir = @datadir@ +datarootdir = @datarootdir@ +docdir = @docdir@ +dvidir = @dvidir@ +exec_prefix = @exec_prefix@ +host = @host@ +host_alias = @host_alias@ +host_cpu = @host_cpu@ +host_os = @host_os@ +host_vendor = @host_vendor@ +htmldir = @htmldir@ +includedir = @includedir@ +infodir = @infodir@ +install_sh = @install_sh@ +libdir = @libdir@ +libexecdir = @libexecdir@ +localedir = @localedir@ +localstatedir = @localstatedir@ +lt_ECHO = @lt_ECHO@ +mandir = @mandir@ +mkdir_p = @mkdir_p@ +oldincludedir = @oldincludedir@ +pdfdir = @pdfdir@ +prefix = @prefix@ +program_transform_name = @program_transform_name@ +psdir = @psdir@ +sbindir = @sbindir@ +sharedstatedir = @sharedstatedir@ +srcdir = @srcdir@ +sysconfdir = @sysconfdir@ +target_alias = @target_alias@ +toolbox = @toolbox@ +top_build_prefix = @top_build_prefix@ +top_builddir = @top_builddir@ +top_srcdir = @top_srcdir@ + +# use nostdinc to turn off -I. and -I.., we do not need them because +# header files are qualified so they can be included in external projects. +AUTOMAKE_OPTIONS = nostdinc +headers = +sources = +vSLAMexample_includedir = . +vSLAMexample_SOURCES = vSLAMexample.cpp Feature2D.cpp landmarkUtils.cpp +#vSLAMexample_DATA = Data/calib.txt +#---------------------------------------------------------------------------------------------------- +# rules to build local programs +#---------------------------------------------------------------------------------------------------- +AM_LDFLAGS = $(BOOST_LDFLAGS) +AM_CPPFLAGS = -I$(boost) -I$(SparseInc) -I$(top_srcdir)/.. +LDADD = ../../libgtsam.la +AM_DEFAULT_SOURCE_EXT = .cpp +all: all-am + +.SUFFIXES: +.SUFFIXES: .cpp .lo .o .obj +$(srcdir)/Makefile.in: $(srcdir)/Makefile.am $(am__configure_deps) + @for dep in $?; do \ + case '$(am__configure_deps)' in \ + *$$dep*) \ + ( cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh ) \ + && { if test -f $@; then exit 0; else break; fi; }; \ + exit 1;; \ + esac; \ + done; \ + echo ' cd $(top_srcdir) && $(AUTOMAKE) --gnu examples/vSLAMexample/Makefile'; \ + $(am__cd) $(top_srcdir) && \ + $(AUTOMAKE) --gnu examples/vSLAMexample/Makefile +.PRECIOUS: Makefile +Makefile: $(srcdir)/Makefile.in $(top_builddir)/config.status + @case '$?' in \ + *config.status*) \ + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh;; \ + *) \ + echo ' cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe)'; \ + cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe);; \ + esac; + +$(top_builddir)/config.status: $(top_srcdir)/configure $(CONFIG_STATUS_DEPENDENCIES) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh + +$(top_srcdir)/configure: $(am__configure_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(ACLOCAL_M4): $(am__aclocal_m4_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(am__aclocal_m4_deps): + +clean-checkPROGRAMS: + @list='$(check_PROGRAMS)'; test -n "$$list" || exit 0; \ + echo " rm -f" $$list; \ + rm -f $$list || exit $$?; \ + test -n "$(EXEEXT)" || exit 0; \ + list=`for p in $$list; do echo "$$p"; done | sed 's/$(EXEEXT)$$//'`; \ + echo " rm -f" $$list; \ + rm -f $$list + +clean-noinstPROGRAMS: + @list='$(noinst_PROGRAMS)'; test -n "$$list" || exit 0; \ + echo " rm -f" $$list; \ + rm -f $$list || exit $$?; \ + test -n "$(EXEEXT)" || exit 0; \ + list=`for p in $$list; do echo "$$p"; done | sed 's/$(EXEEXT)$$//'`; \ + echo " rm -f" $$list; \ + rm -f $$list +vSLAMexample$(EXEEXT): $(vSLAMexample_OBJECTS) $(vSLAMexample_DEPENDENCIES) + @rm -f vSLAMexample$(EXEEXT) + $(CXXLINK) $(vSLAMexample_OBJECTS) $(vSLAMexample_LDADD) $(LIBS) + +mostlyclean-compile: + -rm -f *.$(OBJEXT) + +distclean-compile: + -rm -f *.tab.c + +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/Feature2D.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/landmarkUtils.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/vSLAMexample.Po@am__quote@ + +.cpp.o: +@am__fastdepCXX_TRUE@ $(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCXX_TRUE@ $(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXXCOMPILE) -c -o $@ $< + +.cpp.obj: +@am__fastdepCXX_TRUE@ $(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'` +@am__fastdepCXX_TRUE@ $(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXXCOMPILE) -c -o $@ `$(CYGPATH_W) '$<'` + +.cpp.lo: +@am__fastdepCXX_TRUE@ $(LTCXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCXX_TRUE@ $(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Plo +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=yes @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(LTCXXCOMPILE) -c -o $@ $< + +mostlyclean-libtool: + -rm -f *.lo + +clean-libtool: + -rm -rf .libs _libs + +ID: $(HEADERS) $(SOURCES) $(LISP) $(TAGS_FILES) + list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | \ + $(AWK) '{ files[$$0] = 1; nonempty = 1; } \ + END { if (nonempty) { for (i in files) print i; }; }'`; \ + mkid -fID $$unique +tags: TAGS + +TAGS: $(HEADERS) $(SOURCES) $(TAGS_DEPENDENCIES) \ + $(TAGS_FILES) $(LISP) + set x; \ + here=`pwd`; \ + list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | \ + $(AWK) '{ files[$$0] = 1; nonempty = 1; } \ + END { if (nonempty) { for (i in files) print i; }; }'`; \ + shift; \ + if test -z "$(ETAGS_ARGS)$$*$$unique"; then :; else \ + test -n "$$unique" || unique=$$empty_fix; \ + if test $$# -gt 0; then \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + "$$@" $$unique; \ + else \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + $$unique; \ + fi; \ + fi +ctags: CTAGS +CTAGS: $(HEADERS) $(SOURCES) $(TAGS_DEPENDENCIES) \ + $(TAGS_FILES) $(LISP) + list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | \ + $(AWK) '{ files[$$0] = 1; nonempty = 1; } \ + END { if (nonempty) { for (i in files) print i; }; }'`; \ + test -z "$(CTAGS_ARGS)$$unique" \ + || $(CTAGS) $(CTAGSFLAGS) $(AM_CTAGSFLAGS) $(CTAGS_ARGS) \ + $$unique + +GTAGS: + here=`$(am__cd) $(top_builddir) && pwd` \ + && $(am__cd) $(top_srcdir) \ + && gtags -i $(GTAGS_ARGS) "$$here" + +distclean-tags: + -rm -f TAGS ID GTAGS GRTAGS GSYMS GPATH tags + +distdir: $(DISTFILES) + @srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + topsrcdirstrip=`echo "$(top_srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + list='$(DISTFILES)'; \ + dist_files=`for file in $$list; do echo $$file; done | \ + sed -e "s|^$$srcdirstrip/||;t" \ + -e "s|^$$topsrcdirstrip/|$(top_builddir)/|;t"`; \ + case $$dist_files in \ + */*) $(MKDIR_P) `echo "$$dist_files" | \ + sed '/\//!d;s|^|$(distdir)/|;s,/[^/]*$$,,' | \ + sort -u` ;; \ + esac; \ + for file in $$dist_files; do \ + if test -f $$file || test -d $$file; then d=.; else d=$(srcdir); fi; \ + if test -d $$d/$$file; then \ + dir=`echo "/$$file" | sed -e 's,/[^/]*$$,,'`; \ + if test -d "$(distdir)/$$file"; then \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + if test -d $(srcdir)/$$file && test $$d != $(srcdir); then \ + cp -fpR $(srcdir)/$$file "$(distdir)$$dir" || exit 1; \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + cp -fpR $$d/$$file "$(distdir)$$dir" || exit 1; \ + else \ + test -f "$(distdir)/$$file" \ + || cp -p $$d/$$file "$(distdir)/$$file" \ + || exit 1; \ + fi; \ + done +check-am: all-am + $(MAKE) $(AM_MAKEFLAGS) $(check_PROGRAMS) +check: check-am +all-am: Makefile $(PROGRAMS) +installdirs: +install: install-am +install-exec: install-exec-am +install-data: install-data-am +uninstall: uninstall-am + +install-am: all-am + @$(MAKE) $(AM_MAKEFLAGS) install-exec-am install-data-am + +installcheck: installcheck-am +install-strip: + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + `test -z '$(STRIP)' || \ + echo "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'"` install +mostlyclean-generic: + +clean-generic: + +distclean-generic: + -test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES) + -test . = "$(srcdir)" || test -z "$(CONFIG_CLEAN_VPATH_FILES)" || rm -f $(CONFIG_CLEAN_VPATH_FILES) + +maintainer-clean-generic: + @echo "This command is intended for maintainers to use" + @echo "it deletes files that may require special tools to rebuild." +clean: clean-am + +clean-am: clean-checkPROGRAMS clean-generic clean-libtool \ + clean-noinstPROGRAMS mostlyclean-am + +distclean: distclean-am + -rm -rf ./$(DEPDIR) + -rm -f Makefile +distclean-am: clean-am distclean-compile distclean-generic \ + distclean-tags + +dvi: dvi-am + +dvi-am: + +html: html-am + +html-am: + +info: info-am + +info-am: + +install-data-am: + +install-dvi: install-dvi-am + +install-dvi-am: + +install-exec-am: + +install-html: install-html-am + +install-html-am: + +install-info: install-info-am + +install-info-am: + +install-man: + +install-pdf: install-pdf-am + +install-pdf-am: + +install-ps: install-ps-am + +install-ps-am: + +installcheck-am: + +maintainer-clean: maintainer-clean-am + -rm -rf ./$(DEPDIR) + -rm -f Makefile +maintainer-clean-am: distclean-am maintainer-clean-generic + +mostlyclean: mostlyclean-am + +mostlyclean-am: mostlyclean-compile mostlyclean-generic \ + mostlyclean-libtool + +pdf: pdf-am + +pdf-am: + +ps: ps-am + +ps-am: + +uninstall-am: + +.MAKE: check-am install-am install-strip + +.PHONY: CTAGS GTAGS all all-am check check-am clean \ + clean-checkPROGRAMS clean-generic clean-libtool \ + clean-noinstPROGRAMS ctags distclean distclean-compile \ + distclean-generic distclean-libtool distclean-tags distdir dvi \ + dvi-am html html-am info info-am install install-am \ + install-data install-data-am install-dvi install-dvi-am \ + install-exec install-exec-am install-html install-html-am \ + install-info install-info-am install-man install-pdf \ + install-pdf-am install-ps install-ps-am install-strip \ + installcheck installcheck-am installdirs maintainer-clean \ + maintainer-clean-generic mostlyclean mostlyclean-compile \ + mostlyclean-generic mostlyclean-libtool pdf pdf-am ps ps-am \ + tags uninstall uninstall-am + + +# rule to run an executable +%.run: % $(LDADD) + ./$^ + +# rule to run executable with valgrind +%.valgrind: % $(LDADD) + valgrind ./$^ +#---------------------------------------------------------------------------------------------------- + +# Tell versions [3.59,3.63) of GNU make to not export all variables. +# Otherwise a system limit (for SysV at least) may be exceeded. +.NOEXPORT: diff --git a/examples/vSLAMexample/landmarkUtils.cpp b/examples/vSLAMexample/landmarkUtils.cpp new file mode 100644 index 000000000..cca8eba2e --- /dev/null +++ b/examples/vSLAMexample/landmarkUtils.cpp @@ -0,0 +1,123 @@ +#include "landmarkUtils.h" +#include +#include + +using namespace gtsam; +using namespace std; + +/* ************************************************************************* */ +bool readLandMarks(const char* landmarkFile, std::map& landmarks) +{ + ifstream file(landmarkFile); + if (!file) { + cout << "Cannot read landmark file: " << landmarkFile << endl; + exit(0); + } + + int num; + file >> num; + landmarks.clear(); + for (int i = 0; i> color_id >> x >> y >> z; + landmarks[color_id] = Point3(x, y, z); + } + + file.close(); + return true; +} + +/* ************************************************************************* */ +/** + * Read pose from file, output by Panda3D. + * Warning: row major!!! + */ +gtsam::Pose3 readPose(const char* Fn) +{ + ifstream poseFile(Fn); + if (!poseFile) + { + cout << "Cannot read pose file: " << Fn << endl; + exit(0); + } + + double v[16]; + for (int i = 0; i<16; i++) + poseFile >> v[i]; + poseFile.close(); + + // Because panda3d's camera is z-up, y-view, + // we swap z and y to have y-up, z-view, then negate z to stick with the right-hand rule + //... similar to OpenGL's camera + for (int i = 0; i<3; i++) + { + float t = v[4+i]; + v[4+i] = v[8+i]; + v[8+i] = -t; + } + + ::Vector vec = Vector_(16, v); + + Matrix T = Matrix_(4,4, vec); // column order !!! + + Pose3 pose(T); + return pose; +} +/* ************************************************************************* */ +gtsam::Pose3 readPose(const char* poseFn_pre, const char* poseFn_suf, int poseId) +{ + char poseFn[128]; + sprintf(poseFn, "%s%d%s", poseFn_pre, poseId, poseFn_suf); + return readPose(poseFn); +} + +/* ************************************************************************* */ +gtsam::Cal3_S2 readCalibData(const char* calibFn) +{ + ifstream calibFile(calibFn); + if (!calibFile) + { + cout << "Cannot read calib file: " << calibFn << endl; + exit(0); + } + int imX, imY; + float fx, fy, ox, oy; + calibFile >> imX >> imY >> fx >> fy >> ox >> oy; + calibFile.close(); + + Cal3_S2 K(fx, fy, 0, ox, oy); // skew factor = 0 + return K; +} +/* ************************************************************************* */ +std::vector readFeatures(const char* filename) +{ + ifstream file(filename); + if (!file) + { + cout << "Cannot read feature file: " << filename<< endl; + exit(0); + } + + int numFeatures; + file >> numFeatures ; + + std::vector vFeatures_; + for (size_t i = 0; i < numFeatures; i++) + { + int id; double x, y; + file >> id >> x >> y; + vFeatures_.push_back(Feature2D(id, Point2(x, y))); + } + + file.close(); + return vFeatures_; +} +/* ************************************************************************* */ +std::vector readFeatures(const char* featFn_pre, const char* featFn_suf, int imageId) +{ + char featFn[128]; + sprintf(featFn, "%s%d%s", featFn_pre, imageId, featFn_suf); + return readFeatures(featFn); +} diff --git a/examples/vSLAMexample/landmarkUtils.h b/examples/vSLAMexample/landmarkUtils.h new file mode 100644 index 000000000..7b6a35fb6 --- /dev/null +++ b/examples/vSLAMexample/landmarkUtils.h @@ -0,0 +1,23 @@ +#ifndef LANDMARKUTILS_H +#define LANDMARKUTILS_H + +#include +#include +#include "Feature2D.h" +#include "gtsam/geometry/Pose3.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Cal3_S2.h" + + +bool readLandMarks(const char* landmarkFile, std::map& landmarks); + +gtsam::Pose3 readPose(const char* poseFn); +gtsam::Pose3 readPose(const char* poseFn_pre, const char* poseFn_suf, int poseId); + +gtsam::Cal3_S2 readCalibData(const char* calibFn); + +std::vector readFeatures(const char* filename); +std::vector readFeatures(const char* featFn_pre, const char* featFn_suf, int imageId); + + +#endif // LANDMARKUTILS_H diff --git a/examples/vSLAMexample/vSLAMexample.cpp b/examples/vSLAMexample/vSLAMexample.cpp new file mode 100644 index 000000000..f158a9566 --- /dev/null +++ b/examples/vSLAMexample/vSLAMexample.cpp @@ -0,0 +1,136 @@ +/** + * @file vSLAMexample.cpp + * @brief An vSLAM example for synthesis sequence + * single camera + * @author Duy-Nguyen + */ + +#include +#include +using namespace boost; + +#define GTSAM_MAGIC_KEY + +#include +#include +#include +#include +#include + +#include "landmarkUtils.h" +#include "Feature2D.h" + +using namespace std; +using namespace gtsam; +using namespace gtsam::visualSLAM; +using namespace boost; +typedef NonlinearOptimizer Optimizer; + +/* ************************************************************************* */ +#define CALIB_FILE "Data/calib.txt" +#define LANDMARKS_FILE "Data/landmarks.txt" + +#define POSEFN_PREFIX "Data/ttpy" +#define POSEFN_SUFFIX ".pose" +#define FEATFN_PREFIX "Data/ttpy" +#define FEATFN_SUFFIX ".feat" + +#define NUM_IMAGES 10 +const int ImageIds[NUM_IMAGES] = {10,20,30,40,50,60,70,80,90,100}; + +// Store groundtruth values +map g_landmarks; +vector g_poses; + +/* ************************************************************************* */ +/** + * Setup vSLAM graph + * by adding and associating 2D features (measurements) detected in each image + * with their corresponding landmarks. + */ +Graph setupGraph() +{ + Graph g; + shared_ptrK sK(new Cal3_S2(readCalibData(CALIB_FILE))); + sK->print("Calibration: "); + SharedGaussian sigma(noiseModel::Isotropic::Sigma(2,5.0f)); + + for (size_t i= 0; i features = readFeatures(FEATFN_PREFIX, FEATFN_SUFFIX, ImageIds[i]); + for (size_t j = 0; j::iterator lmit = g_landmarks.begin(); lmit != g_landmarks.end(); lmit++) + initValues.insert( lmit->first, lmit->second ); + + // Initialize camera poses. + for (int i = 0; i graph(new Graph(setupGraph())); + + // Read groundtruth landmarks' positions and poses. These will also be used later as intial estimates. + readLandMarks(LANDMARKS_FILE, g_landmarks); + for (int i = 0; i initialValues(new Values(initializeValues())); + + // Add hard constraint on the first pose, used as fixed prior. + graph->addPoseConstraint(0, g_poses[0]); + + // Create an ordering of the variables + shared_ptr ordering(new Ordering); + char name[4]; + for (size_t i = 0; iprint("Initial estimates: "); + + // Optimize the graph. + Optimizer::verbosityLevel verborsity = Optimizer::ERROR; + Optimizer optimResult = optimizer.levenbergMarquardt(1e-5, 1e-5, verborsity); + optimResult.config()->print("After optimization: "); + +} +/* ************************************************************************* */ + diff --git a/myconfigure b/myconfigure index 23c05be1a..d26570a71 100755 --- a/myconfigure +++ b/myconfigure @@ -1,4 +1,4 @@ -../configure --prefix=$HOME/borg --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ --with-sparse-inc=$HOME/include/SuiteSparse --with-sparse-lib=$HOME/lib/SuiteSparse CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/usr/local/bin/gfilt" CPPFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install +../configure --prefix=$HOME/workspace/gtsam2 --with-toolbox=$HOME/workspace/gtsam2/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/usr/local/bin/gfilt" CPPFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install #../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/usr/local/bin/gfilt" CXXFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG -DNDEBUG" CFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG -DNDEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install #cd build && ../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-fno-inline -g -DNDEBUG -O3" CFLAGS="-fno-inline -g -DNDEBUG -O3" LDFLAGS="-fno-inline -g -DNDEBUG -O3" --disable-static --enable-blas --enable-lapack #cd build && ../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-DNDEBUG -O3" CFLAGS="-g -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static --enable-blas --enable-lapack diff --git a/slam/visualSLAM.h b/slam/visualSLAM.h index 1994b7a8a..3413eb88c 100644 --- a/slam/visualSLAM.h +++ b/slam/visualSLAM.h @@ -27,10 +27,11 @@ #include #include #include +#include namespace gtsam { namespace visualSLAM { - /** + /** * Typedefs that make up the visualSLAM namespace. */ typedef TypedSymbol PoseKey; @@ -42,6 +43,8 @@ namespace gtsam { namespace visualSLAM { typedef NonlinearEquality PoseConstraint; typedef NonlinearEquality PointConstraint; + typedef PriorFactor PosePrior; + /** @@ -182,6 +185,18 @@ namespace gtsam { namespace visualSLAM { push_back(factor); } + /** + * Add a prior on a pose + * @param j index of camera + * @param p to which pose to constrain it to + * @param model uncertainty model of this prior + */ + void addPosePrior(int j, const Pose3& p = Pose3(), const SharedGaussian& model = noiseModel::Unit::Create(1)) { + boost::shared_ptr factor(new PosePrior(j, p, model)); + push_back(factor); + } + + }; // Graph } } // namespaces