Merge remote-tracking branch 'origin/develop' into feature/Similarity

Partial update of Similarity to BAD

Conflicts:
	.gitignore

.gitignore

@@ -1,8 +1,8 @@
 /build*
-/doc*
 *.pyc
 *.DS_Store
 /examples/Data/dubrovnik-3-7-pre-rewritten.txt
 /examples/Data/pose2example-rewritten.txt
 /examples/Data/pose3example-rewritten.txt
-/.settings/
+*.txt.user
+*.txt.user.6d59f0c

.settings/.gitignore

@@ -0,0 +1 @@
+/org.eclipse.cdt.codan.core.prefs

.settings/org.eclipse.cdt.core.prefs

@@ -0,0 +1,6 @@
+eclipse.preferences.version=1
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/delimiter=\:
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/operation=append
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/value=$PATH\:/opt/local/bin
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/append=true
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/appendContributed=true

CppUnitLite/Test.h

@@ -80,6 +80,12 @@ class Test
     testGroup##testName##Instance; \
   void testGroup##testName##Test::run (TestResult& result_)
 
+/**
+ * Use this to disable unwanted tests without commenting them out.
+ */
+#define TEST_DISABLED(testGroup, testName)\
+    void testGroup##testName##Test(TestResult& result_, const std::string& name_)
+
 /*
  * Convention for tests:
  *  - "EXPECT" is a test that will not end execution of the series of tests

GTSAM-Concepts.md

@@ -0,0 +1,206 @@
+GTSAM Concepts
+==============
+
+As discussed in [Generic Programming Techniques](http://www.boost.org/community/generic_programming.html), concepts define
+
+* associated types
+* valid expressions, like functions and values
+* invariants
+* complexity guarantees
+
+Below we discuss the most important concepts use in GTSAM, and after that we discuss how they are implemented/used/enforced.
+
+
+Manifold
+--------
+
+To optimize over continuous types, we assume they are manifolds. This is central to GTSAM and hence discussed in some more detail below.
+
+[Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space.
+
+In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct  `gtsam::traits`. Concept checks are used to check that all required functions are implemented.
+In detail, we ask the following are defined in the traits object:
+
+* values:
+   * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimenionality is only defined at runtime, by specifying the value -1.
+* types: 
+    * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix<double,n,1>`.
+    * `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.  
+    * `ManifoldType`, a pointer back to the type.
+    * `structure_category`, a tag type that defines what requirements the type fulfills, and therefore what requirements this traits class must fulfill. It should be defined to be one of the following: 
+        * `gtsam::traits::manifold_tag` -- Everything in this list is expected
+        * `gtsam::traits::group_tag` -- The functions defined under **Groups** below.
+        * `gtsam::traits::lie_group_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below.
+        * `gtsam::traits::vector_space_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below.
+* valid expressions:
+    * `size_t dim = traits<T>::getDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time.
+    * `v = traits<T>::Local(p,q)`, the chart, from manifold to tangent space, think of it as *q (-) p*, where *p* and *q* are elements of the manifold and the result, *v* is an element of the vector space.
+    * `p = traits<T>::Retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v*, where *p* is an element of the manifold and the result, *v* is an element of the vector space.
+* invariants
+    * `Retract(p, Local(p,q)) == q`
+    * `Local(p, Retract(p, v)) == v`
+
+Group
+-----
+A [group]("http://en.wikipedia.org/wiki/Group_(mathematics)"") should be well known from grade school :-), and provides a type with a composition operation that is closed, associative, has an identity element, and an inverse for each element. The following should be added to the traits class for a group:
+
+* valid expressions:
+    * `r = traits<T>::Compose(p,q)`, where *p*, *q*, and *r* are elements of the manifold. 
+    * `q = traits<T>::Inverse(p)`, where *p* and*q* are elements of the manifold. 
+    * `r = traits<T>::Between(p,q)`, where *p*, *q*, and *r* are elements of the manifold. 
+* static members:
+    * `traits<T>::Identity`, a static const member that represents the group's identity element.
+* invariants:
+    * `Compose(p,Inverse(p)) == Identity`
+    * `Compose(p,Between(p,q)) == q`
+    * `Between(p,q) == Compose(Inverse(p),q)`
+The `gtsam::group::traits` namespace defines the following:
+* values:
+    * `traits<T>::Identity` -- The identity element for this group stored as a static const.
+    * `traits<T>::group_flavor` -- the flavor of this group's `compose()` operator, either:
+         *  `gtsam::traits::group_multiplicative_tag` for multiplicative operator syntax ,or 
+         * `gtsam::traits::group_additive_tag` for additive operator syntax.
+
+We do *not* at this time support more than one composition operator per type. Although mathematically possible, it is hardly ever needed, and the machinery to support it would be burdensome and counter-intuitive. 
+
+Also, a type should provide either multiplication or addition operators depending on the flavor of the operation. To distinguish between the two, we will use a tag (see below).
+
+Lie Group
+---------
+
+A Lie group is both a manifold *and* a group. Hence, a LIE_GROUP type should implements both MANIFOLD and GROUP concepts. 
+However, we now also need to be able to evaluate the derivatives of compose and inverse. 
+Hence, we have the following extra valid static functions defined in the struct `gtsam::traits<T>`:
+
+* `r = traits<T>::Compose(p,q,Hq,Hp)`
+* `q = traits<T>::Inverse(p,Hp)`
+* `r = traits<T>::Between(p,q,Hq,H2p)`
+
+where above the *H* arguments stand for optional Jacobian arguments. 
+That makes it possible to create factors implementing priors (PriorFactor) or relations between two instances of a Lie group type (BetweenFactor).
+
+In addition, a Lie group has a Lie algebra, which affords two extra valid expressions:
+
+* `v = traits<T>::Logmap(p,Hp)`, the log map, with optional Jacobian
+* `p = traits<T>::Expmap(v,Hv)`, the exponential map, with optional Jacobian
+
+Note that in the Lie group case, the usual valid expressions for Retract and Local can be generated automatically, e.g.
+
+```
+    T Retract(p,v,Hp,Hv) {
+      T q = Expmap(v,Hqv);
+      T r = Compose(p,q,Hrp,Hrq);
+      Hv = Hrq * Hqv; // chain rule
+      return r;
+    }
+```
+
+For Lie groups, the `exponential map` above is the most obvious mapping: it 
+associates straight lines in the tangent space with geodesics on the manifold 
+(and it's inverse, the log map). However, there are two cases in which we deviate from this:
+
+However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`)
+
+Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method.
+
+Vector Space
+------------
+
+While vector spaces are in principle also manifolds, it is overkill to think about charts etc. Really, we should simply think about vector addition and subtraction. I.e.where
+
+  * `Identity == 0`
+  * `Inverse(p) == -p`
+  * `Compose(p,q) == p+q`
+  * `Between(p,q) == q-p`
+  * `Local(q) == p-q`   
+  * `Retract(v) == p+v`
+
+This considerably simplifies certain operations. A `VectorSpace` superclass is available to implement the traits. Types that are vector space models include `Matrix`, `Vector`, any fixed or dynamic Eigen Matrix, `Point2`, and `Point3`.
+
+Testable
+--------
+Unit tests heavily depend on the following two functions being defined for all types that need to be tested:
+
+* valid expressions:
+    * `Print(p,s)` where s is an optional string
+    * `Equals(p,q,tol)` where tol is an optional (double) tolerance 
+
+Implementation
+==============
+
+GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the 
+TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts.
+
+`gtsam::traits` is our way to associate these concepts with types, 
+and we also define a limited number of `gtsam::tags` to select the correct implementation
+of certain functions at compile time (tag dispatching). 
+
+Traits
+------
+
+However, a base class is not a good way to implement/check the other concepts, as we would like these
+to apply equally well to types that are outside GTSAM control, e.g., `Eigen::VectorXd`. This is where
+[traits](http://www.boost.org/doc/libs/1_57_0/libs/type_traits/doc/html/boost_typetraits/background.html) come in.
+
+We use Eigen-style or STL-style traits, that define *many* properties at once. 
+
+Note that not everything that makes a concept is defined by traits. Valid expressions such as traits<T>::Compose are
+defined simply as static functions within the traits class.
+Finally, for GTSAM types, it is perfectly acceptable (and even desired) to define associated types as internal types, 
+rather than having to use traits internally.
+
+Concept Checks
+--------------
+
+Boost provides a nice way to check whether a given type satisfies a concept. For example, the following
+
+    BOOST_CONCEPT_ASSERT(IsVectorSpace<Point2>)
+
+asserts that Point2 indeed is a model for the VectorSpace concept.
+
+Future Concepts
+===============
+
+
+Group Action
+------------
+
+Group actions are concepts in and of themselves that can be concept checked (see below).
+In particular, a group can *act* on another space.
+For example, the [cyclic group of order 6](http://en.wikipedia.org/wiki/Cyclic_group) can rotate 2D vectors around the origin:
+
+    q = R(i)*p
+    where R(i) = R(60)^i, where R(60) rotates by 60 degrees
+
+Hence, we formalize by the following extension of the concept:
+
+* valid expressions:
+    * `q = traits<T>::Act(g,p)`, for some instance, *p*,  of a space *S*, that can be acted upon by the group element *g* to produce *q* in *S*.
+    * `q = traits<T>::Act(g,p,Hp)`, if the space acted upon is a continuous differentiable manifold. *
+
+In the latter case, if *S* is an n-dimensional manifold, *Hp* is an output argument that should be 
+filled with the *nxn* Jacobian matrix of the action with respect to a change in *p*. It typically depends
+on the group element *g*, but in most common example will *not* depend on the value of *p*. For example, in 
+the cyclic group example above, we simply have
+
+    Hp = R(i)
+
+Note there is no derivative of the action with respect to a change in g. That will only be defined
+for Lie groups, which we introduce now.
+
+
+Lie Group Action
+----------------
+
+When a Lie group acts on a space, we have two derivatives to care about:
+
+  * `gtasm::manifold::traits<T>::act(g,p,Hg,Hp)`, if the space acted upon is a continuous differentiable manifold.
+
+An example is a *similarity transform* in 3D, which can act on 3D space, like
+
+    q = s*R*p + t
+
+Note that again the derivative in *p*,  *Hp* is simply *s R*, which depends on *g* but not on *p*. 
+The derivative  in *g*,  *Hg*, is in general more complex.
+
+For now, we won't care about Lie groups acting on non-manifolds.

README.md

@@ -40,10 +40,12 @@ Optional prerequisites - used automatically if findable by CMake:
 Additional Information
 ----------------------
 
-Read about important [`GTSAM-Concepts`] here.
+Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
 
-See the [`INSTALL`] file for more detailed installation instructions.
+See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
 
-GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files.
+GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
 
-Please see the [`examples/`](examples) directory and the [`USAGE`] file for examples on how to use GTSAM.
+Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
+
+GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).

THANKS

@@ -1,20 +1,43 @@
-GTSAM was made possible by the efforts of many collaborators at Georgia Tech
+GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech:
 
-Doru Balcan
-Chris Beall
-Alex Cunningham
-Alireza Fathi
-Eohan George
-Viorela Ila
-Yong-Dian Jian
-Michael Kaess
-Kai Ni
-Carlos Nieto
-Duy-Nguyen
-Manohar Paluri
-Christian Potthast
-Richard Roberts
-Grant Schindler
+* Sungtae An
+* Doru Balcan, Bank of America
+* Chris Beall
+* Luca Carlone
+* Alex Cunningham, U Michigan
+* Jing Dong
+* Alireza Fathi, Stanford
+* Eohan George
+* Alex Hagiopol
+* Viorela Ila, Czeck Republic
+* Vadim Indelman, the Technion
+* David Jensen, GTRI
+* Yong-Dian Jian, Baidu
+* Michael Kaess, Carnegie Mellon
+* Zhaoyang Lv
+* Andrew Melim, Oculus Rift
+* Kai Ni, Baidu
+* Carlos Nieto
+* Duy-Nguyen Ta
+* Manohar Paluri, Facebook
+* Christian Potthast, USC
+* Richard Roberts, Google X
+* Grant Schindler, Consultant
+* Natesh Srinivasan
+* Alex Trevor
+* Stephen Williams, BossaNova
+
+at ETH, Zurich
+
+* Paul Furgale
+* Mike Bosse
+* Hannes Sommer
+* Thomas Schneider
+
+at Uni Zurich:
+
+* Christian Forster
 
 Many thanks for your hard work!!!!
+
 Frank Dellaert

USAGE → USAGE.md

@@ -1,6 +1,5 @@
 USAGE - Georgia Tech Smoothing and Mapping library
----------------------------------------------------
-
+===================================
 What is this file?
 
 	This file explains how to make use of the library for common SLAM tasks, 
@@ -34,18 +33,12 @@ The GTSAM library has three primary components necessary for the construction
 of factor graph representation and optimization which users will need to 
 adapt to their particular problem.  
 
-FactorGraph:
-	A factor graph contains a set of variables to solve for (i.e., robot poses,
-	landmark poses, etc.) and a set of constraints between these variables, which 
-	make up factors.  
-Values: 
-	Values is a single object containing labeled values for all of the 
-	variables.  Currently, all variables are labeled with strings, but the type 
-	or organization of the variables can change
-Factors:
-	A nonlinear factor expresses a constraint between variables, which in the
-	SLAM example, is a measurement such as a visual reading on a landmark or
-	odometry.
+* FactorGraph:
+	A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors.  
+* Values: 
+	Values is a single object containing labeled values for all of the variables.  Currently, all variables are labeled with strings, but the type or organization of the variables can change
+* Factors:
+	A nonlinear factor expresses a constraint between variables, which in the SLAM example, is a measurement such as a visual reading on a landmark or odometry.
 
 The library is organized according to the following directory structure:
 
@@ -59,23 +52,3 @@ The library is organized according to the following directory structure:
 
 
 
-VSLAM Example
----------------------------------------------------
-The visual slam example shows a full implementation of a slam system.  The example contains
-derived versions of NonlinearFactor, NonlinearFactorGraph, in classes visualSLAM::ProjectionFactor, 
-visualSLAM::Graph, respectively. The values for the system are stored in the generic 
-Values structure. For definitions and interface, see gtsam/slam/visualSLAM.h. 
-
-The clearest example of the use of the graph to find a solution is in 
-testVSLAM.  The basic process for using graphs is as follows (and can be seen in 
-the test):
-  - Create a NonlinearFactorGraph object (visualSLAM::Graph)
-  - Add factors to the graph (note the use of Boost.shared_ptr here) (visualSLAM::ProjectionFactor)
-  - Create an initial configuration (Values)
-  - Create an elimination ordering of variables (this must include all variables)
-  - Create and initialize a NonlinearOptimizer object (Note that this is a generic 
-      algorithm that does not need to be derived for a particular problem)
-  - Call optimization functions with the optimizer to optimize the graph
-  - Extract an updated values from the optimizer
-
-

cmake/GtsamBuildTypes.cmake

@@ -54,7 +54,7 @@ if(NOT FIRST_PASS_DONE)
 endif()
 
 # Clang on Mac uses a template depth that is less than standard and is too small
-if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
 	if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")
 		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
 	endif()