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
6aba2f1c1c
Changed initial SQP example to use exmap using the new scaling function for VectorConfig
2009-11-20 05:13:32 +00:00
acfe742c29
Added a simple scaling function for VectorConfigs
2009-11-20 05:10:55 +00:00
714edb57f0
Basic implementation of a binary nonlinear constraint, with working linearization of a binary constraint.
2009-11-20 03:50:48 +00:00
ee4a066275
Implemented linearization and equality for unary NonlinearConstraints. Current tests use a scalar example. Split out implementation into a separate implementation file.
2009-11-20 03:04:49 +00:00
4ca1dbf325
generalized most of the update algorithm
2009-11-19 23:25:03 +00:00
429f27550c
Added a NonlinearConstraint and testNonlinearConstraint. There is currently an abstract base class for constraints and a partially implemented unary constraint.
2009-11-19 16:50:18 +00:00
cd913566f2
I now use push_back to add factors to the new factor graph, and called getOrdering to compute a colamd order. I fixed the test so it checks the right order. Unfortunately colamd is a worse ordering, and the two cliques from before are merged into one.
2009-11-19 06:37:06 +00:00
8d8503bd99
constructor with three parents
2009-11-19 06:34:28 +00:00
b2eee54b5a
push_back an entire factor graph
2009-11-19 06:34:07 +00:00
e2a3d1cb8f
Fixed test
2009-11-19 04:52:46 +00:00
f98f49381a
Added an example to testSQP to use for combining graphs
2009-11-18 21:57:59 +00:00
d700cd2cac
Made some small changes to testSQP to simplify the system and improve convergence.
2009-11-18 20:23:07 +00:00
7fc68e2d90
Updated testSQP to use a real SQP implementation as separate factors on the previous problem. SQP now works.
2009-11-18 19:42:14 +00:00
76d0a60b88
commented out all print statements
2009-11-18 18:22:08 +00:00
f40182518a
size() now returns correct nr. of cliques, not number of keys
2009-11-18 18:05:12 +00:00
d0efe2da5e
update almost correct, except for order
2009-11-18 17:40:05 +00:00
23031fbc04
convert clique into factor graph
2009-11-18 17:28:07 +00:00
1edcdb6f80
added SymbolicFactor constructor that takes a SymbolicConditional
2009-11-18 17:27:27 +00:00
c7e76891da
skeleton hardcoded version
2009-11-18 17:03:19 +00:00
bed219c997
no printout
2009-11-18 16:32:45 +00:00
21974f95e1
test for incremental update
2009-11-18 16:31:19 +00:00
bef2efacb7
fixed equals
2009-11-18 16:30:57 +00:00
ceed2ae17d
symbolic tree in testIncremental
2009-11-18 15:37:19 +00:00
f56bf8c79f
added missing include to satisfy stricter compiler; added isam test file
2009-11-17 21:47:42 +00:00
4f7d31986c
added cameraConstraint and corresponding unit test based on same 2 camera/4 landmark example, but now there are constraints on the two cameras and none of the landmarks.
2009-11-17 17:19:54 +00:00
e4eaae4605
Added equals function and serialization capability
2009-11-17 01:19:55 +00:00
d24846d9a0
fixed errors due to shared pointer change in VSLAMFactor
2009-11-17 01:02:55 +00:00
e312252006
Changed camera matrix to a shared pointer
2009-11-17 00:51:27 +00:00
256c094039
Added default constructor and Serialization
2009-11-16 23:49:04 +00:00
20830a1534
Added testSQP demo to show a simple problem solved by Sequential Quadratic Programming. To see the outputs, set verbose=true at the start of the test.
2009-11-16 19:12:56 +00:00
27a47c6535
Constructor now takes Point2
2009-11-13 06:19:21 +00:00
d5ada2a80c
Test now succeeds after adding 3 landmark constraints
...
Also cleaned up a bit
2009-11-13 06:19:08 +00:00
d26abf3ccf
addLandmarkConstraint
2009-11-13 06:18:27 +00:00
309f2151cf
Clean-up
2009-11-13 06:17:59 +00:00
fe974a3e72
Rank test needs to be re-thought
2009-11-13 06:17:19 +00:00
f5fc14c0f4
Fixed bug in revealing rank, even simpler than before.
2009-11-13 06:16:56 +00:00
7c045a0802
Fixed compile errors due to remaining VectorConfigs, and removed div0 warning
2009-11-13 06:15:48 +00:00
17aaae42d8
Refactoring of weightedPseudoInverse, emul, sum
2009-11-13 06:14:55 +00:00
2178589263
Debug printout and variable names
2009-11-13 06:13:58 +00:00
67c2ea8263
NonlinearEquality factor is now working for generic configs. Currently, the feasible point must be specified in the form of a full config with the point in it, but this avoid needing to increase the requirements on the configs.
2009-11-13 02:06:52 +00:00
cba4a536bb
includes Ordering.h
2009-11-13 01:56:09 +00:00
6252ff2497
added testVSLAMGraph unit test. will fail until we can add a constraint to either one of the two cameras.
2009-11-12 18:55:03 +00:00
ef0fc9fbac
added testVSLAMGraph unit test. will fail until we can add a constraint to either one of the two cameras.
2009-11-12 18:54:46 +00:00
03f865d4b1
Used typedefs with similar naming convention.
2009-11-12 18:33:14 +00:00
5857635ca1
Fixed renaming error with NonlinearFactors sometimes becoming NonGaussianFactors
2009-11-12 16:47:12 +00:00
c7b86cec97
Renamed ConditionalGaussian -> GaussianConditional
2009-11-12 16:41:18 +00:00
77a1754b69
Renamed LinearFactor -> GaussianFactor, LinearFactorGraph -> GaussianFactorGraph
2009-11-12 16:16:32 +00:00
1ae6bb4030
Added push_fron convenience method to add ConditionalGaussians into a Bayes net with much less clutter. Modernized some very old tests in the process.
2009-11-12 06:09:03 +00:00
f677341108
Moved inference methods to new compilation unit. Added [factor], and [marginalize] now returns a factor graph.
2009-11-12 04:56:30 +00:00
c85f5445db
Re-organized makefile
2009-11-12 04:54:31 +00:00
81a353dd2c
eliminate is now const
2009-11-12 04:53:28 +00:00
a38ebece1e
New compilation unit that collects generic, templated inference methods that go between factor graphs and Bayes nets. These used to be in BayesNet-inl.h and FactorGraph-inl.h
2009-11-12 04:52:40 +00:00
4a7e05fffa
change all print() to print(string s) and create SymbolicFactor
2009-11-11 22:45:08 +00:00
70efccefbc
VSLAMFactor Testable
2009-11-11 21:09:43 +00:00
8c695a778c
Implemented a rank deficient constraint test in testLinearFactor: note that it is possible to reorder elimination to make a system solvable or rank-deficient
2009-11-11 19:00:39 +00:00
ab65dfb4dd
Adding include file stdio.h to resolve compiling issue when upgrading to Ubuntu 9.10. The printf() compiles on ubuntu 9.04 and mac machines.
2009-11-11 17:15:17 +00:00
f51614813e
Improved weighted eliminate to handle arbitrary linear equality constraints
2009-11-11 14:42:09 +00:00
d585a329dc
Added MATLAB-friendly versions of eliminate and optimize in LinearFactorGraph (with trailing underscore). Also fixed some MATLAB code.
2009-11-11 07:14:13 +00:00
ac840d6f0a
1) DCVectorConfig I believe was obsolete as well as any conversion from and to Vectors, which I removed
...
2) load and flush are gone.
3) I made a VSLAMConfig "Testable".
4) I added an explicit template instantiation to avoid having to include -inl.h files
5) I added some more test code in testVSLAMFactor
2009-11-11 05:14:03 +00:00
00ff066b33
Fixed compile error
2009-11-11 05:13:25 +00:00
8d2d48d252
made removeAndCombineFactors a function, not a method
2009-11-11 05:12:45 +00:00
710d396d0b
cleaned up a few includes to make VSLAMGraph work properly after the move into gtsam.
2009-11-10 22:49:22 +00:00
dc3b85e43d
Todo: make wrapper for GaussianBayesNet. For now eliminate returns void in gtsam.h
2009-11-10 21:15:28 +00:00
08c9718b12
Moving VSLAMConfig into gtsam. VSLAMFactor lived there already, but was using VectorConfig directly... now using VSLAMConfig.
2009-11-10 20:19:00 +00:00
0d928035e0
added empty constructors for serialization, also made sure SymbolicConditional.h was installed
2009-11-10 14:55:07 +00:00
1f15650da0
Added serialization of base class
2009-11-10 14:44:53 +00:00
ddc0173671
Basic handling of constraints now works in factor graphs, assuming there is only one constraint on any given variable.
2009-11-10 04:36:07 +00:00
a7b711db37
Removed constraint variations on graphs/factors
2009-11-10 04:33:39 +00:00
2c37c94b5d
Replaced the householder transform with the weighted system
...
Removed constrained components from makefile, they will disappear shortly
2009-11-09 21:34:20 +00:00
03985d16f6
Fixed equality checking with NaNs in Matrix
...
removed extraneous print statement in Vector
2009-11-09 16:57:30 +00:00
01ee9246d7
fix Matlab toolbox to work with Frank's recent changes to gtsam.
2009-11-09 16:53:13 +00:00
1ae81b59d8
Fixed vector comparisons that will erroneously return true when there are NaN values.
2009-11-09 16:47:37 +00:00
a3de1964d7
BIG CHANGE:
...
1) eliminate methods no longer return a shared pointer. Shared pointers are good for Factors and Conditionals (which are also non-copyable), because these are often passed around under the hood. However, a BayesNet is simple a list of shared pointers and hence does not cost a lot to return as an object (which is compiler-optimized anyway: there is no copy). So, the signature of all eliminate methods changed to simply return a BayesNet<> object (not a shared pointer).
2) GaussianBayesNet::optimize is now replaced by optimize(GaussianBayesNet) and returns a VectorConfig and not a shared pointer
3) GaussianBayesNet and SymbolicBayesNet are now simply typedefs, not derived classes. This is desirable because the BayesTree class uses templated methods that return BayesNet<Conditional>, not a specific BayesNet derived class.
2009-11-09 07:04:26 +00:00
7bd40e836d
Cleaned up and added test for VSLAMFactor
2009-11-09 04:46:34 +00:00
e340178de5
Easy constructor
2009-11-09 04:46:08 +00:00
dd1b023ca9
Fixed special case for root
2009-11-09 04:45:38 +00:00
cc22e82ca6
Joint densities (covariances) now done. Was exceedingly easy :-)
2009-11-09 00:13:44 +00:00
3464c6a36f
Create from a single string
2009-11-08 22:51:29 +00:00
86173b66af
Clique marginal and dramatically simplified single variable marginal.
2009-11-08 22:51:12 +00:00
10e618f360
New function marginals. Also: combine entire Bayes nets with push_back and push_front. And finally: some convenience constructors in GaussianBayesNet.
2009-11-08 22:50:26 +00:00
58007a8167
Working recursive (but not cached) shortcut.
2009-11-08 04:41:01 +00:00
d1b8925617
Construct symbolic version of any Bayes net
2009-11-08 04:39:54 +00:00
532be063f9
new templated function combine
2009-11-08 02:50:19 +00:00
93da9e0f2c
pop_front
2009-11-08 02:49:18 +00:00
cc5a2c3183
Renamed double-templated functions to _eliminate and _eliminateOne, and created FactorGraph-specific eliminateOne methods to make life easier
2009-11-07 21:03:30 +00:00
df3e5f2416
BIG: eliminate and eliminateOne now doubly templated functions, not methods.
...
Minor: Standardized on new shared_ptr naming convention:
shared_factor -> sharedFactor
conditional_ptr -> sharedConditional
node_ptr -> sharedClique
2009-11-07 19:31:39 +00:00
f5d186522e
Standardized on new shared_ptr naming convention
2009-11-07 17:24:05 +00:00
6339b5d19d
Made Node public
2009-11-07 15:58:45 +00:00
7ef9ed950b
Improved performance of matrix scaling in LinearFactor::matrix_augmented
...
Added matrix scaling functions to scale either the rows or columns by values from a vector
2009-11-06 13:43:39 +00:00
cd313e2f82
operator[] to retrieve clique
2009-11-06 05:55:29 +00:00
e1716a39cd
Two changes: LinearFactor::sparse and LinearFactorGraph:sparse, and renamed VariableSet -> Dimensions, which is now a map from keys to integer variable dimensions. Merged in from the "sparse" branch created with Viorela.
2009-11-06 05:43:03 +00:00
fb74ef03b2
Added to LinearFactor a matrix_augmented() function to get a single augmented matrix [A b]
...
Removed extra copying in LinearFactor::eliminate()
2009-11-05 15:08:58 +00:00
Eohan George
Kai Ni
Alex Cunningham
Viorela Ila
Frank Dellaert
Michael Kaess
Chris Beall
Manohar Paluri