Use calcLinearState in pairwise factor creation
Since assignments are overwritten, use same map for each edge
Merge pull request #1 from vene/fixorder

Use calcLinearState in pairwise factor creation