e1388c0f0d
add Ordering-inl.h
2010-01-14 07:56:03 +00:00
dd697a838d
added TupleConfig.h as header
2010-01-14 06:20:34 +00:00
df62213619
RangeFactor works
2010-01-14 06:00:17 +00:00
a552ba4539
Comments/formatting only
2010-01-14 05:59:33 +00:00
8088aea598
Added double as Lie type, needed to remove Lie.h include from Vector.h
2010-01-14 05:58:58 +00:00
1519d029dc
use PredecessorMap instead of map<Key,Key>
2010-01-14 05:24:20 +00:00
8562c00a52
Explicit instantiation to fix link error on Linux, also header to install that was missing from Makefile.am
2010-01-14 05:01:40 +00:00
9bf6409b16
BearingFactor works
2010-01-14 04:35:18 +00:00
32626b3391
bug fix (only worked on Mac)
2010-01-14 03:42:34 +00:00
68d90772e6
ordering is fixed
2010-01-14 03:21:07 +00:00
ac10c440e1
PairConfig is implemented, VSLAMConfig is now a typedef!
2010-01-14 02:58:29 +00:00
6b3e8cf49c
simulated2D now reduced to one .h and .cpp, in its own namespace, better naming, and new-style functions to serve as example
2010-01-14 02:50:06 +00:00
fca2de8f95
fix unit test
2010-01-14 02:21:14 +00:00
7ad114c72f
problem with predecessorMap2Graph
2010-01-14 02:16:29 +00:00
63440f937f
almost fixed oredering except one function call
2010-01-14 02:07:14 +00:00
5a02b36723
Solved some linking problems
2010-01-14 01:44:00 +00:00
6c7dfabe5b
remove template class G and V from predecessorMap2Graph as Frank suggested
2010-01-14 01:25:40 +00:00
3c66861790
fix the graph related functions in FactorGraph as well as its unit tests
2010-01-13 23:59:46 +00:00
93465945e9
Large gtsam refactoring
...
To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.
The following are the biggest changes:
1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.
2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just says something like
if (H) *H = Matrix_(3,6,....);
3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>
4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
2010-01-13 22:25:03 +00:00
22b4912d5e
adding #include<boost/shared_ptr.hpp> to graph-inl.h because this dependency doesnt seem to be working without it on gcc 4.3.3
2010-01-13 19:36:52 +00:00
c0bc7e6c10
fix a bug in composePoses that switched the order of compose parameters
2010-01-13 06:02:18 +00:00
2a094b7491
return shared_ptr in composePoses
2010-01-13 02:09:16 +00:00
8d28763339
Vector and Matrix initialization optimization
2010-01-12 16:12:31 +00:00
4369cd2d92
1) add a compose function that compose all the poses in a factor graph given the spanning tree
...
2) add a new graph-inl.h which takes care of all the boost graph related functions to prevent the copy and paste across several classes
2010-01-12 16:12:25 +00:00
bea55b5f5b
Fixed double map lookup
2010-01-12 16:11:24 +00:00
fcb7c024a7
bearing functions and derivatives
2010-01-12 02:10:42 +00:00
4b459c45c3
return identity
2010-01-12 02:09:43 +00:00
5163684077
norm
2010-01-12 02:09:03 +00:00
1aed18717b
combined evaluation and derivatives
2010-01-12 02:08:41 +00:00
2c8d8dbde4
NonlinearOptimizer constructor now requires shared_ptr to prevent storing pointers to stack and temporary variables. Code that uses it will need to be modified, but CitySLAM is already done.
2010-01-11 20:17:28 +00:00
40889e8f50
added an absolution threshold $epsilon_abs$ to conjugateGradients. added utility functions to several class to have the same interface which can be used by template functions
2010-01-11 08:32:59 +00:00
1b53a240b9
removed prints
2010-01-11 01:09:07 +00:00
e9b0bade03
Access to keys
2010-01-11 00:53:53 +00:00
71fe372fb3
test successive iterates
2010-01-11 00:53:43 +00:00
aa2aa4149c
Special case for trace==1 in Rot3 logmap
2010-01-10 23:36:37 +00:00
b0fd5396ad
Bug fix in Rot3 for logmap(identity)
2010-01-10 22:58:30 +00:00
1dcc864d97
Comment cleanup to /** style and adding global print functions
2010-01-10 22:41:23 +00:00
78a579a24a
add the missing #pragma once
2010-01-10 22:15:34 +00:00
f8be2c2626
dim, expmap, logmap works
2010-01-10 21:53:38 +00:00
64eca2d550
addConstraint
2010-01-10 19:25:19 +00:00
d11b2e6bd5
Added omitted headers
2010-01-10 18:37:55 +00:00
88ae5380a8
Removed Vector-specific versions of NumericalDerivative, instead Vector is now a Lie object
2010-01-10 18:23:47 +00:00
1a96ef41cf
Created Pose3Config, Pose3Graph, and tested loop closure optimization.
2010-01-10 18:21:20 +00:00
2fe02dbaa7
Moved optimization tests to testPose2Graph
2010-01-10 18:20:10 +00:00
edb68b3b60
Fixed typo in LieConfig expmap(Vector)
2010-01-10 17:48:55 +00:00
33ba34a1f6
Minimized headers included in Lie.h, removed print from testLieConfig
2010-01-10 17:26:44 +00:00
8d84078036
Loop closure tested
2010-01-10 17:02:49 +00:00
12caabb667
Method to add a factor without having to do shared factor dance
2010-01-10 17:02:35 +00:00
120f1301c2
operator[]
2010-01-10 17:01:48 +00:00
710bce5cc4
Pose2Config is now simply a typedef (note that for linking specific template instantiations were needed).
2010-01-10 16:17:55 +00:00
4a21fb9387
Fixed 2nd expmap template, which now creates a VectorConfig and calls the first template
2010-01-10 16:16:03 +00:00
65e4dc1342
pose2Circle
2010-01-10 15:46:18 +00:00
43b2facd10
Now Pose3Config == LieConfig
2010-01-10 14:59:39 +00:00
92c58e50a1
Small changes, mainly const correctness
2010-01-10 14:59:22 +00:00
c957672656
Small to change to work well with LieConfig
2010-01-10 14:55:19 +00:00
794ffd3b2f
Fixed significant bug in Pose2Factor, where we forgot to multiply the error with the square root inverse covariance.
2010-01-10 13:55:55 +00:00
8061f29ec5
added test clarifying Ax-b \approx h(x)-z
2010-01-10 13:54:34 +00:00
f1f21a8589
inverted sign of square root to make inv sqrt positive for simple covariance matrices
2010-01-10 13:53:31 +00:00
bf315781af
defined global functions in Lie.h rather than here
2010-01-10 12:35:41 +00:00
6bcb9d08d6
Unit tests for correct "Agrawal06iros" versions: they are indeed correct.
2010-01-10 12:26:31 +00:00
30367e35fb
Much faster compund rotation using Justin's (indeed correct) formula
2010-01-10 12:25:46 +00:00
d5eade62ef
Moved expmap and logmap to .cpp, also tested correct "Agrawal06iros" versions, and they are indeed correct (see testPose3).
2010-01-10 12:24:31 +00:00
ec1b57ed08
Added Lie required functions (trivial) for Vector. Not in Vector.h as this becomes circular.
2010-01-10 12:22:59 +00:00
4fa53a1f79
Merging lieconfig branch - LieConfig works but currently requires Lie objects to have a global print function.
2010-01-10 06:35:16 +00:00
6a7987fa91
fix Pose2Prior problem and adding prior factor when splitting factor graph
2010-01-09 23:39:11 +00:00
2b82ff65e7
Deriving Lie objects from a base class Lie<T>, which provides member functions to access global functions, for use in MATLAB.
2010-01-09 23:15:06 +00:00
d015b31799
more meaningful error messages for matrix operations
2010-01-09 07:06:29 +00:00
cc358fb2ce
removed print statements
2010-01-09 02:37:58 +00:00
5fbb820341
fixed comments and tolerances
2010-01-09 02:27:49 +00:00
6851b31fad
Added static methods to construct rotations, coded up convention for yaw-pitch-roll, modernized RQ
2010-01-09 00:03:47 +00:00
d4f92c7bb6
pass FactorGraph as reference into split
2010-01-08 20:27:10 +00:00
06b7f8ee04
add split to FactorGraph
2010-01-08 19:07:46 +00:00
5dc237eeea
BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.
2010-01-08 15:32:45 +00:00
215ff24d0f
generate ordering by breadth-first-search, a lot faster than before
2010-01-08 04:12:23 +00:00
8d22b3aca2
Just refactoring
2010-01-08 03:38:05 +00:00
f684becf1f
Fixed Dbetween1
2010-01-08 03:06:03 +00:00
19a3e228d7
Large refactoring - made several Lie group functions global, which used to be member functions, to treat Lie groups more uniformly. Also created Lie.h, and a preprocessor flag in numericalDerivative to change the coordinate frame derivatives are reported in. gtsam and easylib build and pass unit tests, but this will probably break other projects, which will require a few small changes to work again. Email coming in a few minutes to describe the changes.
2010-01-08 00:40:17 +00:00
a845b3d30a
generate ordering from the spanning tree
2010-01-07 08:00:54 +00:00
6298a45050
use prim algorithm instead of kruskal for finding MST, which yields the parent map.
2010-01-07 06:25:54 +00:00
20c6f29823
Moved control-related components to separate library
2010-01-06 20:01:34 +00:00
b20ed42134
log and between
2010-01-06 19:29:41 +00:00
f77da96caf
Modernized and tested RQ and added Rot3::ypr. Note this yaw-pitch-roll is different from ML version, which is focused on cameras. Let the fun begin...
2010-01-06 15:52:43 +00:00
9845a5ae37
add findMinimumSpanningTree to FactorGraph
2010-01-06 11:15:37 +00:00
cd644e75a5
GaussianBayesNet::matrix now properly divides in sigmas
2010-01-05 14:14:49 +00:00
22d66df766
added manifold test
2010-01-05 14:13:51 +00:00
39334ea9e0
Update MATLAB wrapper for earlier Pose2* changes
2010-01-04 18:07:24 +00:00
46cc85e351
added size and clear to pose2config
2010-01-03 07:25:59 +00:00
052149771a
relinearizing factors corresponding to contaminated cliques
2010-01-03 04:57:35 +00:00
cade0e7780
fix timing test linking problem by using noinst_PROGRAMS
2010-01-03 00:23:16 +00:00
68159fb5e4
corrected SubgraphPreconditioner case
2010-01-02 20:24:59 +00:00
f82b46f96c
Sped up rodriguez and replaced slow implementation with call to fast implementation. Also moved some functions to cpp.
2010-01-02 14:28:18 +00:00
a1c55b9f62
Updated Pose2Config to expected coding standard
2010-01-02 10:33:33 +00:00
ff06cd757f
moved all "Urban" factors and tests to (cmake-based) CitySLAM project
2009-12-31 16:54:23 +00:00
a1d14ba2ae
Added Bayes Net and Subgraph preconditioners to gtsam (developed in CitySLAM project)
2009-12-31 12:56:47 +00:00
730f4a546f
Added planar graph with easy subtree
2009-12-31 12:55:51 +00:00
07cc95e4c4
Comments and formatting
2009-12-31 12:55:16 +00:00
eaa8176de1
correct behavior when entire columns are zero
2009-12-31 10:30:44 +00:00
46a65d89df
using ediv_ where needed
2009-12-31 10:30:06 +00:00
f7d5b517a4
ediv_ is same as ediv except returns 0 for 0/0 division, needed for dealing with constraints
2009-12-31 10:29:25 +00:00
3fbc857a9e
fixed behavior of error for sigma==0
2009-12-31 10:28:43 +00:00
9b327d1a9d
added reset
2009-12-31 10:27:39 +00:00
bf85f10de7
subtraction
2009-12-31 10:27:16 +00:00
b4e65e9631
progress in ISAM2, but unit test still disabled
2009-12-31 05:35:08 +00:00
afc3028ad5
Fixed bug in testPose2Factor optimize unit test where only the first expected value was checked.
2009-12-30 18:41:33 +00:00
793a9d58ae
Errors is now a list
2009-12-30 17:13:36 +00:00
f80ac5d7d5
backSubstitute functions (unit-tested in CitySLAM) for preconditioning
2009-12-30 17:13:24 +00:00
2368f3605a
getReference
2009-12-30 14:53:40 +00:00
15bb00683a
transpose backsubstitute on upper-triangular matrix
2009-12-30 13:20:16 +00:00
42fca8c399
nonlinear update, all but rhs/config
2009-12-30 04:27:14 +00:00
0b9451bc4b
Initializing libtool correctly in gtsam (see email to frankcvs with same subject)
2009-12-29 20:01:38 +00:00
737a2f1e05
unit test enabled again and working
2009-12-29 14:54:45 +00:00
c43cd425ab
new back-substitution functions that can do unit triangular solves
2009-12-29 13:59:34 +00:00
75ab62a729
ISAM2 (nonlinear ISAM) partially there, unit test currently disabled
2009-12-29 05:57:05 +00:00
8d4a029665
rhs method
2009-12-28 18:46:01 +00:00
543d3fcd65
System now a class (gradient is method)
2009-12-28 17:28:48 +00:00
0c0b73042b
template now exposed in iterative.h, and has its own implementation file
2009-12-28 16:26:16 +00:00
5ac304aff3
correct convergence criterion, verbose flag
2009-12-28 16:15:26 +00:00
674ae9d030
steepest descent now all implemented in iterative.h/cpp
2009-12-28 12:37:34 +00:00
bc27afc49f
CGD method now uses CGD template in iterative.cpp
2009-12-28 10:48:48 +00:00
863ee58c0f
new iterative.h/cpp compilation unit
2009-12-28 09:56:58 +00:00
d9fd502656
moved CG tests from testGFG to testIterative
2009-12-28 09:44:30 +00:00
1fac98b4cb
Errors:dot, VectorConfig::operator*/-, as a result Conjugate Gradient Descent template now works for factor graphs
2009-12-28 08:15:09 +00:00
5dfd1921e1
template nightmare resolved, all tests working
2009-12-27 23:15:36 +00:00
266fc56dea
Gradient using new operator^ and errors method
2009-12-27 12:13:31 +00:00
7d1428de60
Added Errors class and operator* for GaussianFactor and GaussianFactorGraph. Also moved a few functions to cpp.
2009-12-26 22:48:41 +00:00
6614434b83
System version of CG
2009-12-26 21:25:45 +00:00
8d3918e7f9
Typo made testPose2Factor not build on Linux
2009-12-26 19:31:02 +00:00
f3965b07ca
Conjugate Gradient Descent template (in progress)
2009-12-26 15:06:54 +00:00
886c7dcdcc
formatting and comments
2009-12-26 15:06:21 +00:00
8dbf79706e
commented out two broken tests
2009-12-26 15:05:55 +00:00
4403b51fcd
isam functionality separated into ISAM class, still within BayesTree
2009-12-26 04:44:13 +00:00
2cb52694b5
Multiply with transpose sugar A'*v = A^v
2009-12-25 17:52:58 +00:00
80ff7e6d26
uncommented urban In Makefile.am as it prevents linking; added Pose2Prior; ISAM2/planar now working
2009-12-24 23:44:08 +00:00
92b60a8543
Made 'between' derivatives in the tangent space of the solution instead of tangent space of identity, this makes Pose2 an "origin-free" manifold.
2009-12-21 16:43:23 +00:00
d0b757da48
remove method to set factor to NULL
2009-12-20 21:21:01 +00:00
3158a5c7c1
Rework UrbanMeasurement to be more efficient, less duplicate code. Add real unit tests for the class
2009-12-20 14:59:37 +00:00
d362fa13d8
print is more intuitive using reverse order
2009-12-20 11:56:33 +00:00
18c44e0d1f
Comments only
2009-12-20 10:32:34 +00:00
7012fd2857
UrbanMeasurement first pass at implementation. Still need to add test data, sort out some details.
2009-12-18 19:43:55 +00:00
82825a30bf
Fixed unit test broken by fix in Pose2Factor
2009-12-18 14:42:19 +00:00
b14094cb1f
testUrbanOdometry
2009-12-18 07:15:19 +00:00
d11d674c01
addOdometry now works
2009-12-18 06:55:44 +00:00
c987ab397c
error_vectors
2009-12-18 06:55:24 +00:00
bf504d30db
new concatVectors call
2009-12-18 06:11:19 +00:00
ac72d1cc22
renamed transformPose_to to transform_to, composeTransform to compose, and changed nr. of arguments of Dtransform_to2 (to two)
2009-12-18 05:36:53 +00:00
eaf27af92d
error_vector and error unit-tested
2009-12-18 05:18:31 +00:00
edb72d305f
better error message, formatting
2009-12-18 05:16:09 +00:00
0194926dc2
transpose not needed
2009-12-18 05:14:34 +00:00
c9bd327961
formatting
2009-12-18 05:14:08 +00:00
e2bc13a2a6
Renaming gradients -> jacobians
2009-12-18 03:05:47 +00:00
a3deb992c4
use BOOST_FOREACH
2009-12-18 02:48:21 +00:00
a1918056a5
Changed NonlinearConstraints to take cost and gradient functions that do not take the list of keys. Tests have been reconstructed using boost::bind
2009-12-18 02:39:02 +00:00
5a0bc8ab53
remove binaries
2009-12-18 02:30:05 +00:00
cf1fde7bda
UrbanFactor, UrbanGraph.cpp, template instantiations
2009-12-18 02:27:50 +00:00
7d0de77fc6
Changed argument ordering for constraints
2009-12-18 01:40:29 +00:00
7dfc67860c
unit tests for new Urban* classes
2009-12-18 01:24:28 +00:00
d75a602546
Removed failed test testSQPOptimizer
2009-12-18 01:24:19 +00:00
ff817551de
Added FactorGraph::replace(index, factor) to replace one factor with another (used in FrameSLAM).
2009-12-18 00:13:38 +00:00
3c0ae0ec1c
Math fix in linearize() and error_vector()
2009-12-18 00:10:20 +00:00
7161878285
Small changes: standardized constructors, added log() and unit tests, removed +,- ops, angle() changed to theta(), print functions
2009-12-18 00:09:54 +00:00
d61d92c0e7
UrbanGraph class and testUrbanGraph TODOs
2009-12-17 21:23:50 +00:00
68de3ae715
better comments
2009-12-17 19:34:27 +00:00
dc9a966fdd
moved shared pointer declaration to Cal3_S2.h
2009-12-17 10:02:43 +00:00
3326759f0b
Removed extra ;
2009-12-16 21:41:33 +00:00
c45f64657f
Changes to make Pose2Config work with NonlinearOptimizer
2009-12-16 21:40:41 +00:00
83c95f2343
Changed constructor used from Pose2(t,R) to Pose2(R,t)
2009-12-16 21:39:03 +00:00
9efac7b3fb
Some re-naming and re-formatting only
2009-12-15 05:34:49 +00:00
94f986bbe7
Indentation
2009-12-15 00:00:02 +00:00
a7a5e5e816
small performance improvement by using a pointer to address r
2009-12-14 16:56:50 +00:00
71bc9ca0f7
Added unit tests that were missing
2009-12-14 15:45:45 +00:00
971b575715
Fix duplicate definition error by adding 'inline' to checkConvergence
2009-12-14 06:10:51 +00:00
2270c7d787
BayesTree animation
2009-12-14 06:01:15 +00:00
c4b60bfd65
Closing # 118: Made Pose2 use Rot2, added 'compose' for Pose2, made slight modifications to Rot2. Also added additional unit tests. This change shouldn't break anything but let me know if it does.
2009-12-14 03:02:05 +00:00
0116b3a515
severe bug fixed in updating of Bayestree
2009-12-13 20:54:48 +00:00
d5d70d7751
gradientDescent_ and conjugateGradientDescent_ works in matlab
2009-12-13 03:02:14 +00:00
6e3f36dc63
Trying different orderings
2009-12-12 12:53:39 +00:00
2914d663de
No print in tests
2009-12-12 06:18:59 +00:00
c38683cd64
Sped up and fixed (?) sparse
2009-12-12 06:18:29 +00:00
99533f286f
Vanilla Conjugate Gradient Descent works
2009-12-12 04:44:34 +00:00
2a2963b7dd
Added a numerical gradient calculation test
2009-12-12 04:13:12 +00:00
56a4172480
Fixed printing in matlab
2009-12-12 04:12:24 +00:00
be10fc2a90
18% performance boost by using pointer to address destination matrix A
2009-12-11 23:59:37 +00:00
631248f902
no print yet
2009-12-11 23:16:57 +00:00
570c6a3c71
print method for Pose2Graph
2009-12-11 23:00:52 +00:00
8a9f05059c
implement getOrdering_ to work in matlab
2009-12-11 22:53:48 +00:00
a1e90af90f
exmap that takes Vector
2009-12-11 22:43:34 +00:00
137291b2c9
Vector-like operators
2009-12-11 21:38:08 +00:00
e46cfbd7c4
fixed bug in sum
2009-12-11 21:37:52 +00:00
bca489384e
dot product
2009-12-11 21:37:40 +00:00
76b55aba7d
linearize method for Pose2Graph works in Matlab!!
2009-12-11 21:34:08 +00:00
03304ac421
testPose2Graph tests linearize
2009-12-11 19:32:46 +00:00
a66f08a5e0
testing new alphaFactor
2009-12-11 18:03:43 +00:00
f91a1f0192
error_vector
2009-12-11 17:42:54 +00:00
abc268a13d
quick cleanup - expmap comments
2009-12-11 14:22:35 +00:00
16e1f6e56d
use Vector_ for conciseness
2009-12-11 05:28:19 +00:00
726858145a
gradient descent (with fixed nr. of iterations, choosing optimal step size)
2009-12-11 04:59:05 +00:00
299fcf1e04
scale is const !
2009-12-11 04:57:31 +00:00
c57501eddd
fix macro so GTSAM_PRINT(*foo) works
2009-12-11 04:57:14 +00:00
7151e7b24a
new constructor from vector
2009-12-11 04:56:29 +00:00
4af2382105
Gradient at solution is zero !
2009-12-11 02:59:09 +00:00
532ea0ae36
fixed weird error that several of us have been experiencing with GaussianBayesTree.h appearing twice in the install command. fixed by moving Symbolic inference section above Inference Section.
2009-12-11 01:38:45 +00:00
1a96534016
add Pose2Graph
2009-12-10 23:45:38 +00:00
e7a912bd3b
Calculate gradient of factor graph objective function.
2009-12-10 20:19:15 +00:00
67e1897e47
Get and [] now return references, new imperative add method
2009-12-10 20:17:11 +00:00
81ce0bef88
New operator for left-multiplication of vector
2009-12-10 20:16:40 +00:00
2df0e49e9b
optimize works for GaussianBayessTree
2009-12-10 17:00:11 +00:00
f44af12836
Renamed variable
2009-12-10 15:34:09 +00:00
d26664d975
Convenience functions for adding factors
2009-12-10 15:33:52 +00:00
e0274dab59
Pose2Config constructor
2009-12-10 04:12:41 +00:00
072dea81b0
rename Pose2Constraint using Pose2Factor
2009-12-09 23:43:01 +00:00
cee3f2a355
fixed recursive inclusion. Was giving error when you build gtsam
2009-12-09 22:53:44 +00:00
db0d7286c2
Fixed Michael's c++ trubbles.
2009-12-09 22:15:38 +00:00
72ba1bee24
separate cpp
2009-12-09 22:02:50 +00:00
7d4de3ec20
Added a 1D manifold implementation of 2D rotations. The new representation stores (cos theta, sin theta) rather than theta itself, ensuring that (a) rotate and unrotate do not call cos/sin, (b) same for all derivatives of rotate and unrotate, (c) when you call angle(), you always get the standardized answer given by atan2. The idea is that we will use this new type in Pose2 instead of angle.
2009-12-09 21:50:27 +00:00
91370a9d7a
added public iterator typedefs to VSLAMConfig to make it easier for other code to consume the optimization results.
2009-12-09 21:47:23 +00:00
1ec93f05cd
added a test case which uses getOrdering instead of defining the order manually.
2009-12-09 21:22:16 +00:00
a9dc57db2c
2009-12-09 21:00:53 +00:00
ffa009d3ff
accidentially checked in
2009-12-09 20:56:03 +00:00
34e913e236
optimization over cliques started
2009-12-09 20:49:04 +00:00
638b174541
Pose2Constraint works in matlab
2009-12-09 19:55:25 +00:00
4200271cf4
GaussianBayesTree added, testBayesTree split
2009-12-09 19:39:25 +00:00
a4a552ea86
fixed bug concerning variable keys not being saved.
2009-12-09 19:13:02 +00:00
3b3c76b273
cleaned up, and removed Testable here because it's already provided by base class FactorGraph
2009-12-09 19:11:45 +00:00
b87aa58c1f
add Pose2Constraint class
2009-12-09 17:29:43 +00:00
f764772106
added getOrdering unit test
2009-12-09 15:25:50 +00:00
e43548d03a
A couple more "missing" .hpp files for newer gcc :-)
2009-12-08 22:49:57 +00:00
9bbea6dcbd
some additional includes needed with newer compiler
2009-12-08 21:40:09 +00:00
d7c67532bf
Changed format of Matrix::vector_scale() to switch arguments
2009-12-08 21:10:38 +00:00
066b80e02e
Pose2Constraint, initial check in with working linearize
2009-12-08 20:48:39 +00:00
16da0895a8
square_root_inverse
2009-12-08 20:48:13 +00:00
d1ab2c7288
transform_to and between, with derivatives, unit-tested
2009-12-08 19:12:20 +00:00
6ead19648f
Modernized Pose2 constraint and added numerical derivative check
2009-12-08 14:02:56 +00:00
152fa0069a
fixed expected, actual nomenclature in assert_equal: always use (expected, equal) order
2009-12-08 13:53:33 +00:00
95260281d1
add testFunctions (h, H1, H2) for Pose Constraint
2009-12-07 23:17:03 +00:00
53af03368f
Fixed indentation
2009-12-07 22:51:49 +00:00
e244e22c6a
added probability function that computes the probability of a BinaryBayesNet given a config.
2009-12-07 07:12:01 +00:00
115d9a8adf
replaced pow with bit shift operator, changed constructor to take p(x|parents) and created cpt inside the constructor.
2009-12-07 03:25:25 +00:00
28eb550781
Used a map for config
2009-12-07 00:49:13 +00:00
b9e15ee789
Added cpt to BinaryConditional
2009-12-06 23:28:46 +00:00
60a3a21d5a
Binary Bayes Net, incomplete
2009-12-06 21:46:46 +00:00
2a4e90a283
Re-factored append_factor to use FOREACH_PAIR
2009-12-05 02:00:20 +00:00
e98081f92c
changes to append_factor which give more than 2x speedup on my data. Matrices in As are no longer copied/edited/replaced each time they need to be changed, but created once and then updated in place.
2009-12-04 21:03:32 +00:00
ae9789c9ee
Cleaner printing with tabs after each element
2009-12-02 23:01:49 +00:00
5bde26a885
odprintf is an auxiliary function to printf for Win32 compatibility
2009-12-02 19:37:54 +00:00
e26acc0d8d
Changed nonlinear constraints to use boost.bind to handle arbitrary function objects for evaluating cost, useful for parameterizing cost functions.
2009-12-01 19:45:47 +00:00
c00252a8f6
add method combine to GaussianFactorGraph in gtsam.h
2009-12-01 16:01:29 +00:00
a3ce3f31c8
Cleanup with NonlinearConstraints to make the active() function do thresholding for inequality constraints, rather than constraint function itself.
...
testSQP now has all tests active and passing.
Added greaterThanOrEqual() for vector comparison.
2009-11-30 17:36:34 +00:00
1799f59388
Added a function for the SQPOptimizer that will iterate until convergence. At the moment, the convergence conditions are quite simple (error below threshold or too many iterations). The system does, however, strictly limit the number of iterations.
2009-11-28 22:49:14 +00:00
1525253123
Inequality Constraints now work in a simple obstacle-avoidance demo in testSQPOptimizer. It should be noted that convergence conditions are still not implemented.
2009-11-28 21:44:07 +00:00
395e4ae3f1
Fixed sign issue on nonlinear constraints, so that that it is not necessary to flip the sign on the delta configs before using exmap.
2009-11-28 21:00:09 +00:00
0ff7e3a5d9
Changed interface on constraint g(x) and grad_g(x) functions to take a list of keys, so that all of the variables in a factor can be used as necessary. Moved g(x) into base NonlinearConstraint class and some cleanup.
2009-11-28 19:18:02 +00:00
107c6846fb
Added basic inequality constraint handling to NonlinearConstraint. Demo still in progress, but now constraints are explicitly handled as either equality or inequality constraints, with an active() function to determine if it is necessary to use the constraint.
...
Created testOrdering for new subtract() function to remove parts of an ordering.
2009-11-28 18:35:36 +00:00
f3e825767b
NonlinearConstraints now handle inactive constraints (when the state is already in the feasible region) by returning empty factors on linearization.
2009-11-27 18:42:01 +00:00
cf6474c99b
Added a graph implementation for multi-robot control systems. Currently can only really constrain the ends of a trajectory.
...
Made a number of fixes and updates to the ControlConfig as well.
2009-11-27 17:59:03 +00:00
2a2e11d05d
Added the config and units for a robot control system example that keeps a position and velocity model of a robot's motion.
2009-11-26 05:52:17 +00:00
43e4c66407
Added more detail to VSLAM test that diverges.
2009-11-24 16:03:57 +00:00
d788e9833d
Added tests to ensure that configs will only update existing values when using exmap and not add new ones if there are more values in the delta config. VectorConfig and VSLAMConfig both do this properly now.
2009-11-24 15:12:59 +00:00
e3e79e3888
Added lagrange access function to the SQPOptimizer
...
Added a test testSQP to try changing the initial Lagrange multiplier values to find a way around the stability problem, but there is no effect.
2009-11-24 14:44:46 +00:00
140e79572f
Added a test to testSQP to check VSLAM solution stability when the initial config is away from the ground truth. The test without the nonlinear constraints has the same stability problem, and both the constrained and unconstrained versions are in place and disabled.
2009-11-24 14:14:03 +00:00
fec280db8e
Added a nonlinear constraint version of the stereo SQP example, but the test when the initial configuration is incorrect is not stable and the points continue moving away indefinitely.
2009-11-24 06:39:27 +00:00
31856ce598
SQPOptimizer can now do its own initialization of the Lagrange multipliers.
...
Cleaned up NonlinearConstraint
2009-11-23 22:10:52 +00:00
a5515d9d57
SQPoptimizer can now perform optimization, and has a test using the linear map-warping example.
2009-11-23 21:25:31 +00:00
ac315082aa
new example from RSS sqrtSAM in testBayesTree
2009-11-23 20:11:10 +00:00
0b333630e7
Added an SQPOptimizer class with accompanying test - currently doesn't do much
2009-11-23 19:57:35 +00:00
d15a7a073c
SymbolicFactor works in Matlab
2009-11-23 19:45:09 +00:00
36877532ad
Added ground truth for VSLAM example in testSQP
2009-11-23 16:45:53 +00:00
f272e2f6fe
Added a second smoother test
2009-11-23 05:27:37 +00:00
9bac438dbe
Play nice with BOOST_FOREACH: non-const begin/end. Also removed buggy clear().
2009-11-23 05:27:19 +00:00
10a8ac34ec
iSAM smoother test
2009-11-23 01:16:58 +00:00
4951a25453
More efficient removeTop
2009-11-23 00:02:06 +00:00
35af122e83
Added a new version of removeTop and simplified update
2009-11-22 23:50:01 +00:00
7d384488e3
BayesTree::update generalized and moved from testIncremental to BayesTree; testIncremental removed
2009-11-22 23:35:13 +00:00
64a43b0492
removeTop now does not bomb when a clique does not exist: it just does nothing
2009-11-22 22:59:56 +00:00
4e74edba72
Some simplification
2009-11-22 22:39:32 +00:00
ab117947d7
update now takes a factor graph, rather than an individual factor
2009-11-22 22:26:24 +00:00
a3de1df5a4
adding orphans assuming that the first separator key refers to the parent; GTSAM_PRINT define
2009-11-22 21:18:31 +00:00
198f73a05f
Insert conditionals straight into the topless bayesTree
2009-11-22 18:40:55 +00:00
32db0595f4
using removeTop now
2009-11-22 18:27:14 +00:00
952798e751
fixed orphans bug
2009-11-22 18:22:17 +00:00
9da2f16f4d
Removed copy/paste in unit tests
2009-11-22 18:12:29 +00:00
d4aaa5d114
Added two more removePath tests and fixed bug in removeTop. But orphan list is still incorrect.
2009-11-22 18:06:28 +00:00
45292f7bd6
Cliques implemented; unit test on orphans for removePath; bug fix in removePath/orphans
2009-11-22 17:40:24 +00:00
53754ccbb8
removeTop with one factor implemented and one test works (orphans not yet checked)
2009-11-22 17:34:59 +00:00
6f11c0803b
Cliques class
2009-11-22 16:46:29 +00:00
7ce62f1626
Some template magic and boost:tie cleared up the compile issues in removePath
2009-11-22 16:04:51 +00:00
4449cfd30c
remove_path now also returns list of orphaned subtree roots; compile problem...
2009-11-21 23:41:43 +00:00
82aae3c161
Some re-factoring (using BOOST_FOREACH to loop over string list), root is now NULL after removing root clique
2009-11-21 21:48:10 +00:00
1fd0404ec9
Cleanup in testSQP, also demoed the unary constraint, as well.
2009-11-21 19:05:24 +00:00
00b5b25591
Awesome recursive version of removePath
2009-11-21 06:14:53 +00:00
072846a70f
moved addClique and removeClique implementations to inl.h,
...
removePath takes clique argument now
2009-11-21 06:07:46 +00:00
286d826119
removed print
2009-11-21 06:06:30 +00:00
b113194e58
While watching TV :-) added some constructors for more readable tests.
2009-11-21 04:59:50 +00:00
a0a250750b
compare config
2009-11-21 04:22:59 +00:00
0286bc27eb
removePath mostly working
2009-11-21 03:38:13 +00:00
08f5b074a0
Added verbosity selector for the SQP demo to remove printouts every time it runs
2009-11-20 14:29:11 +00:00
715dc4474d
functional STL magic
2009-11-20 14:18:26 +00:00
d234365d53
involves is const
2009-11-20 14:17:52 +00:00
5f848f272b
SQP demo that moves maps into the correct reference frames is now working using the NonlinearConstraint machinery.
2009-11-20 14:05:21 +00:00
e440767db9
Use [involves], and now updating orphan list
2009-11-20 05:23:47 +00:00
55e85fd8a5
[involves] checks whether variable with that key is involved in any factor
2009-11-20 05:14:34 +00:00
Kai Ni
Frank Dellaert
Richard Roberts
Michael Kaess
John Rogers
Manohar Paluri
Viorela Ila
Alex Cunningham
justinca
Chris Beall
Eohan George