- Add vSLAMexample
- Add addPosePrior in visualSLAM.h - Don't know how to install/copy Data folderrelease/4.3a0
Duy-Nguyen Ta
parent
2b3f9b92bd
commit
d55e72ae53
|
|
@ -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])
|
||||
|
|
|
|||
|
|
@ -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
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
800 600 -1119.61507797 1119.61507797 399.5 299.5
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
-1 0 0 0
|
||||
0 -0.707107 -0.707107 0
|
||||
0 -0.707107 0.707107 0
|
||||
0 100 100 1
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
#include "Feature2D.h"
|
||||
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
#ifndef FEATURE2D_H
|
||||
#define FEATURE2D_H
|
||||
|
||||
#include "gtsam/geometry/Point2.h"
|
||||
#include <iostream>
|
||||
|
||||
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
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
#include "FeatureDetector.h"
|
||||
#include "landmarkUtils.h"
|
||||
#include <fstream>
|
||||
#include <stdio.h>
|
||||
|
||||
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<vColors_.size(); i++)
|
||||
if (vColors_[i] == color)
|
||||
return i;
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::vector<Feature2D> FeatureDetector::detect(const cv::Mat& image)
|
||||
{
|
||||
vFeatures_.clear();
|
||||
|
||||
RGBColor BLACK(0,0,0);
|
||||
map<int, int> numPixels; // <colorId, numpixel found>
|
||||
for (int y = 0; y<image.rows; y++) {
|
||||
for (int x = 0; x<image.cols; x++)
|
||||
{
|
||||
if (image.at<RGBColor>(y,x) == BLACK)
|
||||
continue;
|
||||
|
||||
RGBColor color = image.at<RGBColor>(y,x);
|
||||
int t = color[2]; color[2] = color[0]; color[0] = t;
|
||||
int colorId = findColorId(color);
|
||||
|
||||
if (colorId == -1)
|
||||
continue;
|
||||
|
||||
map<int, int>::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<vFeatures_.size(); k++)
|
||||
if (vFeatures_[k].m_id == colorId)
|
||||
break;
|
||||
vFeatures_[k].m_p = (vFeatures_[k].m_p*numPixels[colorId] + Point2(x,y))/(numPixels[colorId]+1);
|
||||
numPixels[colorId]++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return vFeatures_;
|
||||
}
|
||||
|
||||
void FeatureDetector::write(const char* filename) const
|
||||
{
|
||||
ofstream file(filename);
|
||||
file << vFeatures_.size() << endl;
|
||||
for (size_t i = 0; i<vFeatures_.size(); i++)
|
||||
file << vFeatures_[i].m_id << " " << vFeatures_[i].m_p.x() << " " << vFeatures_[i].m_p.y() << endl;
|
||||
file.close();
|
||||
}
|
||||
|
||||
std::vector<Feature2D>& 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_;
|
||||
}
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
#ifndef FEATUREDETECTOR_H
|
||||
#define FEATUREDETECTOR_H
|
||||
|
||||
#include "Feature2D.h"
|
||||
#include <vector>
|
||||
#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<Feature2D> vFeatures_;
|
||||
|
||||
public:
|
||||
std::vector<Feature2D>& read(const char* filename);
|
||||
};
|
||||
|
||||
#endif // FEATUREDETECTOR_H
|
||||
|
|
@ -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 ./$^
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
|
|
@ -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:
|
||||
|
|
@ -0,0 +1,123 @@
|
|||
#include "landmarkUtils.h"
|
||||
#include <fstream>
|
||||
#include <cstdio>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool readLandMarks(const char* landmarkFile, std::map<int, Point3>& 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<num; i++)
|
||||
{
|
||||
int color_id;
|
||||
float x, y, z;
|
||||
file >> 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<Feature2D> readFeatures(const char* filename)
|
||||
{
|
||||
ifstream file(filename);
|
||||
if (!file)
|
||||
{
|
||||
cout << "Cannot read feature file: " << filename<< endl;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
int numFeatures;
|
||||
file >> numFeatures ;
|
||||
|
||||
std::vector<Feature2D> 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<Feature2D> 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);
|
||||
}
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
#ifndef LANDMARKUTILS_H
|
||||
#define LANDMARKUTILS_H
|
||||
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#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<int, gtsam::Point3>& 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<Feature2D> readFeatures(const char* filename);
|
||||
std::vector<Feature2D> readFeatures(const char* featFn_pre, const char* featFn_suf, int imageId);
|
||||
|
||||
|
||||
#endif // LANDMARKUTILS_H
|
||||
|
|
@ -0,0 +1,136 @@
|
|||
/**
|
||||
* @file vSLAMexample.cpp
|
||||
* @brief An vSLAM example for synthesis sequence
|
||||
* single camera
|
||||
* @author Duy-Nguyen
|
||||
*/
|
||||
|
||||
#include <gtsam/CppUnitLite/TestHarness.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
using namespace boost;
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
||||
#include <gtsam/inference/graph-inl.h>
|
||||
#include <gtsam/slam/visualSLAM.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
|
||||
#include "landmarkUtils.h"
|
||||
#include "Feature2D.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace gtsam::visualSLAM;
|
||||
using namespace boost;
|
||||
typedef NonlinearOptimizer<Graph,Values> 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<int, Point3> g_landmarks;
|
||||
vector<Pose3> 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<NUM_IMAGES; i++)
|
||||
{
|
||||
std::vector<Feature2D> features = readFeatures(FEATFN_PREFIX, FEATFN_SUFFIX, ImageIds[i]);
|
||||
for (size_t j = 0; j<features.size(); j++)
|
||||
g.addMeasurement(features[j].m_p, sigma, i, features[j].m_id, sK);
|
||||
}
|
||||
|
||||
return g;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Read initial values.
|
||||
* Note: These are ground-truth values, but we just use them as initial estimates.
|
||||
*/
|
||||
Values initializeValues()
|
||||
{
|
||||
Values initValues;
|
||||
|
||||
// Initialize landmarks 3D positions.
|
||||
for (map<int, Point3>::iterator lmit = g_landmarks.begin(); lmit != g_landmarks.end(); lmit++)
|
||||
initValues.insert( lmit->first, lmit->second );
|
||||
|
||||
// Initialize camera poses.
|
||||
for (int i = 0; i<NUM_IMAGES; i++)
|
||||
initValues.insert(i, g_poses[i]);
|
||||
|
||||
return initValues;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main()
|
||||
{
|
||||
shared_ptr<Graph> 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<NUM_IMAGES; i++)
|
||||
{
|
||||
Pose3 pose = readPose(POSEFN_PREFIX, POSEFN_SUFFIX, ImageIds[i]) ;
|
||||
g_poses.push_back( pose );
|
||||
}
|
||||
|
||||
// Create an initial Values structure using groundtruth as the initial estimates
|
||||
boost::shared_ptr<Values> 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> ordering(new Ordering);
|
||||
char name[4];
|
||||
for (size_t i = 0; i<g_landmarks.size(); i++)
|
||||
{
|
||||
sprintf(name, "l%d", i); // "li"
|
||||
*ordering += name;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i<NUM_IMAGES; i++)
|
||||
{
|
||||
sprintf(name, "x%d", i); // "xj"
|
||||
*ordering += name;
|
||||
}
|
||||
|
||||
// Create an optimizer and check its error
|
||||
// We expect the initial to be zero because Values is the ground truth
|
||||
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
|
||||
Optimizer optimizer(graph, initialValues, solver);
|
||||
cout << "Initial error: " << optimizer.error() << endl;
|
||||
optimizer.config()->print("Initial estimates: ");
|
||||
|
||||
// Optimize the graph.
|
||||
Optimizer::verbosityLevel verborsity = Optimizer::ERROR;
|
||||
Optimizer optimResult = optimizer.levenbergMarquardt(1e-5, 1e-5, verborsity);
|
||||
optimResult.config()->print("After optimization: ");
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@
|
|||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/TupleValues.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
|
||||
namespace gtsam { namespace visualSLAM {
|
||||
|
||||
|
|
@ -42,6 +43,8 @@ namespace gtsam { namespace visualSLAM {
|
|||
|
||||
typedef NonlinearEquality<Values, PoseKey> PoseConstraint;
|
||||
typedef NonlinearEquality<Values, PointKey> PointConstraint;
|
||||
typedef PriorFactor<Values, PoseKey> 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<PosePrior> factor(new PosePrior(j, p, model));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
|
||||
}; // Graph
|
||||
|
||||
} } // namespaces
|
||||
|
|
|
|||
Loading…
Reference in New Issue