Merge remote-tracking branch 'origin/develop' into fix/msvc2017
# Conflicts: # gtsam/geometry/tests/testCameraSet.cpp # gtsam/inference/Ordering.hrelease/4.3a0
Frank Dellaert
commit
a014fd2f22
|
|
@ -0,0 +1,325 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Kinematics of three-link manipulator with GTSAM poses and product of exponential maps.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
import unittest
|
||||
from functools import reduce
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
|
||||
|
||||
import gtsam
|
||||
import gtsam.utils.plot as gtsam_plot
|
||||
from gtsam import Pose2
|
||||
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create 3D double numpy array."""
|
||||
return np.array([x, y, z], dtype=np.float)
|
||||
|
||||
|
||||
def compose(*poses):
|
||||
"""Compose all Pose2 transforms given as arguments from left to right."""
|
||||
return reduce((lambda x, y: x.compose(y)), poses)
|
||||
|
||||
|
||||
def vee(M):
|
||||
"""Pose2 vee operator."""
|
||||
return vector3(M[0, 2], M[1, 2], M[1, 0])
|
||||
|
||||
|
||||
def delta(g0, g1):
|
||||
"""Difference between x,y,,theta components of SE(2) poses."""
|
||||
return vector3(g1.x() - g0.x(), g1.y() - g0.y(), g1.theta() - g0.theta())
|
||||
|
||||
|
||||
def trajectory(g0, g1, N=20):
|
||||
""" Create an interpolated trajectory in SE(2), treating x,y, and theta separately.
|
||||
g0 and g1 are the initial and final pose, respectively.
|
||||
N is the number of *intervals*
|
||||
Returns N+1 poses
|
||||
"""
|
||||
e = delta(g0, g1)
|
||||
return [Pose2(g0.x()+e[0]*t, g0.y()+e[1]*t, g0.theta()+e[2]*t) for t in np.linspace(0, 1, N)]
|
||||
|
||||
|
||||
class ThreeLinkArm(object):
|
||||
"""Three-link arm class."""
|
||||
|
||||
def __init__(self):
|
||||
self.L1 = 3.5
|
||||
self.L2 = 3.5
|
||||
self.L3 = 2.5
|
||||
self.xi1 = vector3(0, 0, 1)
|
||||
self.xi2 = vector3(self.L1, 0, 1)
|
||||
self.xi3 = vector3(self.L1+self.L2, 0, 1)
|
||||
self.sXt0 = Pose2(0, self.L1+self.L2 + self.L3, math.radians(90))
|
||||
|
||||
def fk(self, q):
|
||||
""" Forward kinematics.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
sXl1 = Pose2(0, 0, math.radians(90))
|
||||
l1Zl1 = Pose2(0, 0, q[0])
|
||||
l1Xl2 = Pose2(self.L1, 0, 0)
|
||||
l2Zl2 = Pose2(0, 0, q[1])
|
||||
l2Xl3 = Pose2(self.L2, 0, 0)
|
||||
l3Zl3 = Pose2(0, 0, q[2])
|
||||
l3Xt = Pose2(self.L3, 0, 0)
|
||||
return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)
|
||||
|
||||
def jacobian(self, q):
|
||||
""" Calculate manipulator Jacobian.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
a = q[0]+q[1]
|
||||
b = a+q[2]
|
||||
return np.array([[-self.L1*math.cos(q[0]) - self.L2*math.cos(a)-self.L3*math.cos(b),
|
||||
-self.L1*math.cos(a)-self.L3*math.cos(b),
|
||||
- self.L3*math.cos(b)],
|
||||
[-self.L1*math.sin(q[0]) - self.L2*math.sin(a)-self.L3*math.sin(b),
|
||||
-self.L1*math.sin(a)-self.L3*math.sin(b),
|
||||
- self.L3*math.sin(b)],
|
||||
[1, 1, 1]], np.float)
|
||||
|
||||
def poe(self, q):
|
||||
""" Forward kinematics.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
|
||||
l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
|
||||
l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
|
||||
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
|
||||
|
||||
def con(self, q):
|
||||
""" Forward kinematics, conjugation form.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
def expmap(x, y, theta):
|
||||
"""Implement exponential map via conjugation with axis (x,y)."""
|
||||
return compose(Pose2(x, y, 0), Pose2(0, 0, theta), Pose2(-x, -y, 0))
|
||||
|
||||
l1Zl1 = expmap(0.0, 0.0, q[0])
|
||||
l2Zl2 = expmap(0.0, self.L1, q[1])
|
||||
l3Zl3 = expmap(0.0, 7.0, q[2])
|
||||
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
|
||||
|
||||
def ik(self, sTt_desired, e=1e-9):
|
||||
""" Inverse kinematics.
|
||||
Takes desired Pose2 of tool T with respect to base S.
|
||||
Optional: mu, gradient descent rate; e: error norm threshold
|
||||
"""
|
||||
q = np.radians(vector3(30, -30, 45)) # well within workspace
|
||||
error = vector3(100, 100, 100)
|
||||
|
||||
while np.linalg.norm(error) > e:
|
||||
error = delta(sTt_desired, self.fk(q))
|
||||
J = self.jacobian(q)
|
||||
q -= np.dot(np.linalg.pinv(J), error)
|
||||
|
||||
# return result in interval [-pi,pi)
|
||||
return np.remainder(q+math.pi, 2*math.pi)-math.pi
|
||||
|
||||
def manipulator_jacobian(self, q):
|
||||
""" Calculate manipulator Jacobian.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
Returns the manipulator Jacobian of differential twists. When multiplied with
|
||||
a vector of joint velocities, will yield a single differential twist which is
|
||||
the spatial velocity d(sTt)/dt * inv(sTt) of the end-effector pose.
|
||||
Just like always, differential twists can be hatted and multiplied with spatial
|
||||
coordinates of a point to give the spatial velocity of the point.
|
||||
"""
|
||||
l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
|
||||
l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
|
||||
# l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
|
||||
|
||||
p1 = self.xi1
|
||||
# p1 = Pose2().Adjoint(self.xi1)
|
||||
|
||||
sTl1 = l1Zl1
|
||||
p2 = sTl1.Adjoint(self.xi2)
|
||||
|
||||
sTl2 = compose(l1Zl1, l2Zl2)
|
||||
p3 = sTl2.Adjoint(self.xi3)
|
||||
|
||||
differential_twists = [p1, p2, p3]
|
||||
return np.stack(differential_twists, axis=1)
|
||||
|
||||
def plot(self, fignum, q):
|
||||
""" Plot arm.
|
||||
Takes figure number, and numpy array of joint angles, in radians.
|
||||
"""
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca()
|
||||
|
||||
sXl1 = Pose2(0, 0, math.radians(90))
|
||||
t = sXl1.translation()
|
||||
p1 = np.array([t.x(), t.y()])
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sXl1)
|
||||
|
||||
def plot_line(p, g, color):
|
||||
t = g.translation()
|
||||
q = np.array([t.x(), t.y()])
|
||||
line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], color)
|
||||
return q
|
||||
|
||||
l1Zl1 = Pose2(0, 0, q[0])
|
||||
l1Xl2 = Pose2(self.L1, 0, 0)
|
||||
sTl2 = compose(sXl1, l1Zl1, l1Xl2)
|
||||
p2 = plot_line(p1, sTl2, 'r-')
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sTl2)
|
||||
|
||||
l2Zl2 = Pose2(0, 0, q[1])
|
||||
l2Xl3 = Pose2(self.L2, 0, 0)
|
||||
sTl3 = compose(sTl2, l2Zl2, l2Xl3)
|
||||
p3 = plot_line(p2, sTl3, 'g-')
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sTl3)
|
||||
|
||||
l3Zl3 = Pose2(0, 0, q[2])
|
||||
l3Xt = Pose2(self.L3, 0, 0)
|
||||
sTt = compose(sTl3, l3Zl3, l3Xt)
|
||||
plot_line(p3, sTt, 'b-')
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sTt)
|
||||
|
||||
|
||||
# Create common example configurations.
|
||||
Q0 = vector3(0, 0, 0)
|
||||
Q1 = np.radians(vector3(-30, -45, -90))
|
||||
Q2 = np.radians(vector3(-90, 90, 0))
|
||||
|
||||
|
||||
class TestPose2SLAMExample(unittest.TestCase):
|
||||
"""Unit tests for functions used below."""
|
||||
|
||||
def setUp(self):
|
||||
self.arm = ThreeLinkArm()
|
||||
|
||||
def assertPose2Equals(self, actual, expected, tol=1e-2):
|
||||
"""Helper function that prints out actual and expected if not equal."""
|
||||
equal = actual.equals(expected, tol)
|
||||
if not equal:
|
||||
raise self.failureException(
|
||||
"Poses are not equal:\n{}!={}".format(actual, expected))
|
||||
|
||||
def test_fk_arm(self):
|
||||
"""Make sure forward kinematics is correct for some known test configurations."""
|
||||
# at rest
|
||||
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
|
||||
sTt = self.arm.fk(Q0)
|
||||
self.assertIsInstance(sTt, Pose2)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
# -30, -45, -90
|
||||
expected = Pose2(5.78, 1.52, math.radians(-75))
|
||||
sTt = self.arm.fk(Q1)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
def test_jacobian(self):
|
||||
"""Test Jacobian calculation."""
|
||||
# at rest
|
||||
expected = np.array([[-9.5, -6, -2.5], [0, 0, 0], [1, 1, 1]], np.float)
|
||||
J = self.arm.jacobian(Q0)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
# at -90, 90, 0
|
||||
expected = np.array([[-6, -6, -2.5], [3.5, 0, 0], [1, 1, 1]], np.float)
|
||||
J = self.arm.jacobian(Q2)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
def test_con_arm(self):
|
||||
"""Make sure POE is correct for some known test configurations."""
|
||||
# at rest
|
||||
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
|
||||
sTt = self.arm.con(Q0)
|
||||
self.assertIsInstance(sTt, Pose2)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
# -30, -45, -90
|
||||
expected = Pose2(5.78, 1.52, math.radians(-75))
|
||||
sTt = self.arm.con(Q1)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
def test_poe_arm(self):
|
||||
"""Make sure POE is correct for some known test configurations."""
|
||||
# at rest
|
||||
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
|
||||
sTt = self.arm.poe(Q0)
|
||||
self.assertIsInstance(sTt, Pose2)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
# -30, -45, -90
|
||||
expected = Pose2(5.78, 1.52, math.radians(-75))
|
||||
sTt = self.arm.poe(Q1)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
def test_ik(self):
|
||||
"""Check iterative inverse kinematics function."""
|
||||
# at rest
|
||||
actual = self.arm.ik(Pose2(0, 2*3.5 + 2.5, math.radians(90)))
|
||||
np.testing.assert_array_almost_equal(actual, Q0, decimal=2)
|
||||
|
||||
# -30, -45, -90
|
||||
sTt_desired = Pose2(5.78, 1.52, math.radians(-75))
|
||||
actual = self.arm.ik(sTt_desired)
|
||||
self.assertPose2Equals(self.arm.fk(actual), sTt_desired)
|
||||
np.testing.assert_array_almost_equal(actual, Q1, decimal=2)
|
||||
|
||||
def test_manipulator_jacobian(self):
|
||||
"""Test Jacobian calculation."""
|
||||
# at rest
|
||||
expected = np.array([[0, 3.5, 7], [0, 0, 0], [1, 1, 1]], np.float)
|
||||
J = self.arm.manipulator_jacobian(Q0)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
# at -90, 90, 0
|
||||
expected = np.array(
|
||||
[[0, 0, 3.5], [0, -3.5, -3.5], [1, 1, 1]], np.float)
|
||||
J = self.arm.manipulator_jacobian(Q2)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
|
||||
def run_example():
|
||||
""" Use trajectory interpolation and then trajectory tracking a la Murray
|
||||
to move a 3-link arm on a straight line.
|
||||
"""
|
||||
arm = ThreeLinkArm()
|
||||
q = np.radians(vector3(30, -30, 45))
|
||||
sTt_initial = arm.fk(q)
|
||||
sTt_goal = Pose2(2.4, 4.3, math.radians(0))
|
||||
poses = trajectory(sTt_initial, sTt_goal, 50)
|
||||
|
||||
fignum = 0
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca()
|
||||
axes.set_xlim(-5, 5)
|
||||
axes.set_ylim(0, 10)
|
||||
gtsam_plot.plot_pose2(fignum, arm.fk(q))
|
||||
|
||||
for pose in poses:
|
||||
sTt = arm.fk(q)
|
||||
error = delta(sTt, pose)
|
||||
J = arm.jacobian(q)
|
||||
q += np.dot(np.linalg.inv(J), error)
|
||||
arm.plot(fignum, q)
|
||||
plt.pause(0.01)
|
||||
|
||||
plt.pause(10)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
run_example()
|
||||
unittest.main()
|
||||
|
|
@ -251,10 +251,10 @@ void runIncremental()
|
|||
Key firstPose;
|
||||
while(nextMeasurement < datasetMeasurements.size())
|
||||
{
|
||||
if(BetweenFactor<Pose>::shared_ptr measurement =
|
||||
if(BetweenFactor<Pose>::shared_ptr factor =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
|
||||
{
|
||||
Key key1 = measurement->key1(), key2 = measurement->key2();
|
||||
Key key1 = factor->key1(), key2 = factor->key2();
|
||||
if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
|
||||
// We found an odometry starting at firstStep
|
||||
firstPose = std::min(key1, key2);
|
||||
|
|
@ -302,52 +302,53 @@ void runIncremental()
|
|||
|
||||
NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
|
||||
|
||||
if(BetweenFactor<Pose>::shared_ptr measurement =
|
||||
if(BetweenFactor<Pose>::shared_ptr factor =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
|
||||
{
|
||||
// Stop collecting measurements that are for future steps
|
||||
if(measurement->key1() > step || measurement->key2() > step)
|
||||
if(factor->key1() > step || factor->key2() > step)
|
||||
break;
|
||||
|
||||
// Require that one of the nodes is the current one
|
||||
if(measurement->key1() != step && measurement->key2() != step)
|
||||
if(factor->key1() != step && factor->key2() != step)
|
||||
throw runtime_error("Problem in data file, out-of-sequence measurements");
|
||||
|
||||
// Add a new factor
|
||||
newFactors.push_back(measurement);
|
||||
newFactors.push_back(factor);
|
||||
const auto& measured = factor->measured();
|
||||
|
||||
// Initialize the new variable
|
||||
if(measurement->key1() > measurement->key2()) {
|
||||
if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
|
||||
if(factor->key1() > factor->key2()) {
|
||||
if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
|
||||
if(step == 1)
|
||||
newVariables.insert(measurement->key1(), measurement->measured().inverse());
|
||||
newVariables.insert(factor->key1(), measured.inverse());
|
||||
else {
|
||||
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
|
||||
newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
|
||||
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2());
|
||||
newVariables.insert(factor->key1(), prevPose * measured.inverse());
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if(!newVariables.exists(measurement->key2())) { // Only need to check newVariables since loop closures come after odometry
|
||||
if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry
|
||||
if(step == 1)
|
||||
newVariables.insert(measurement->key2(), measurement->measured());
|
||||
newVariables.insert(factor->key2(), measured);
|
||||
else {
|
||||
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
|
||||
newVariables.insert(measurement->key2(), prevPose * measurement->measured());
|
||||
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1());
|
||||
newVariables.insert(factor->key2(), prevPose * measured);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
|
||||
else if(BearingRangeFactor<Pose, Point2>::shared_ptr factor =
|
||||
boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
|
||||
{
|
||||
Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1];
|
||||
Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];
|
||||
|
||||
// Stop collecting measurements that are for future steps
|
||||
if(poseKey > step)
|
||||
throw runtime_error("Problem in data file, out-of-sequence measurements");
|
||||
|
||||
// Add new factor
|
||||
newFactors.push_back(measurement);
|
||||
newFactors.push_back(factor);
|
||||
|
||||
// Initialize new landmark
|
||||
if(!isam2.getLinearizationPoint().exists(lmKey))
|
||||
|
|
@ -357,8 +358,9 @@ void runIncremental()
|
|||
pose = isam2.calculateEstimate<Pose>(poseKey);
|
||||
else
|
||||
pose = newVariables.at<Pose>(poseKey);
|
||||
Rot2 measuredBearing = measurement->measured().first;
|
||||
double measuredRange = measurement->measured().second;
|
||||
const auto& measured = factor->measured();
|
||||
Rot2 measuredBearing = measured.bearing();
|
||||
double measuredRange = measured.range();
|
||||
newVariables.insert(lmKey,
|
||||
pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
|
||||
}
|
||||
|
|
@ -427,7 +429,7 @@ void runIncremental()
|
|||
// for (Key key12: boost::adaptors::reverse(values.keys())) {
|
||||
// if(i != j) {
|
||||
// gttic_(jointMarginalInformation);
|
||||
// std::vector<Key> keys(2);
|
||||
// KeyVector keys(2);
|
||||
// keys[0] = key1;
|
||||
// keys[1] = key2;
|
||||
// JointMarginal info = marginals.jointMarginalInformation(keys);
|
||||
|
|
@ -522,7 +524,7 @@ void runCompare()
|
|||
|
||||
// Check solution for equality
|
||||
cout << "Comparing solutions..." << endl;
|
||||
vector<Key> missingKeys;
|
||||
KeyVector missingKeys;
|
||||
br::set_symmetric_difference(soln1.keys(), soln2.keys(), std::back_inserter(missingKeys));
|
||||
if(!missingKeys.empty()) {
|
||||
cout << " Keys unique to one solution file: ";
|
||||
|
|
@ -533,7 +535,7 @@ void runCompare()
|
|||
}
|
||||
cout << endl;
|
||||
}
|
||||
vector<Key> commonKeys;
|
||||
KeyVector commonKeys;
|
||||
br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
|
||||
double maxDiff = 0.0;
|
||||
for(Key j: commonKeys)
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ struct general_matrix_matrix_triangular_product<Index,Scalar,LhsStorageOrder,Con
|
|||
static EIGEN_STRONG_INLINE void run(Index size, Index depth,const Scalar* lhs, Index lhsStride, \
|
||||
const Scalar* rhs, Index rhsStride, Scalar* res, Index resStride, Scalar alpha, level3_blocking<Scalar, Scalar>& blocking) \
|
||||
{ \
|
||||
if ( lhs==rhs && ((UpLo&(Lower|Upper)==UpLo)) ) { \
|
||||
if ( lhs==rhs && ((UpLo&(Lower|Upper))==UpLo) ) { \
|
||||
general_matrix_matrix_rankupdate<Index,Scalar,LhsStorageOrder,ConjugateLhs,ColMajor,UpLo> \
|
||||
::run(size,depth,lhs,lhsStride,rhs,rhsStride,res,resStride,alpha,blocking); \
|
||||
} else { \
|
||||
|
|
|
|||
|
|
@ -11,88 +11,26 @@
|
|||
|
||||
/**
|
||||
* @file FastVector.h
|
||||
* @brief A thin wrapper around std::vector that uses boost's pool_allocator.
|
||||
* @brief A thin wrapper around std::vector that uses a custom allocator.
|
||||
* @author Richard Roberts
|
||||
* @author Frank Dellaert
|
||||
* @date Feb 9, 2011
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/FastDefaultAllocator.h>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <boost/serialization/vector.hpp>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* FastVector is a thin wrapper around std::vector that uses the boost
|
||||
* pool_allocator instead of the default STL allocator. This is just a
|
||||
* convenience to avoid having lengthy types in the code. Through timing,
|
||||
* we've seen that the pool_allocator can lead to speedups of several %
|
||||
* FastVector is a type alias to a std::vector with a custom memory allocator.
|
||||
* The particular allocator depends on GTSAM's cmake configuration.
|
||||
* @addtogroup base
|
||||
*/
|
||||
template<typename VALUE>
|
||||
class FastVector: public std::vector<VALUE,
|
||||
typename internal::FastDefaultVectorAllocator<VALUE>::type> {
|
||||
template <typename T>
|
||||
using FastVector =
|
||||
std::vector<T, typename internal::FastDefaultVectorAllocator<T>::type>;
|
||||
|
||||
public:
|
||||
|
||||
typedef std::vector<VALUE,
|
||||
typename internal::FastDefaultVectorAllocator<VALUE>::type> Base;
|
||||
|
||||
/** Default constructor */
|
||||
FastVector() {
|
||||
}
|
||||
|
||||
/** Constructor from size */
|
||||
explicit FastVector(size_t size) :
|
||||
Base(size) {
|
||||
}
|
||||
|
||||
/** Constructor from size and initial values */
|
||||
explicit FastVector(size_t size, const VALUE& initial) :
|
||||
Base(size, initial) {
|
||||
}
|
||||
|
||||
/** Constructor from a range, passes through to base class */
|
||||
template<typename INPUTITERATOR>
|
||||
explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) {
|
||||
// This if statement works around a bug in boost pool allocator and/or
|
||||
// STL vector where if the size is zero, the pool allocator will allocate
|
||||
// huge amounts of memory.
|
||||
if (first != last)
|
||||
Base::assign(first, last);
|
||||
}
|
||||
|
||||
/** Copy constructor from the base class */
|
||||
FastVector(const Base& x) :
|
||||
Base(x) {
|
||||
}
|
||||
|
||||
/** Copy constructor from a standard STL container */
|
||||
template<typename ALLOCATOR>
|
||||
FastVector(const std::vector<VALUE, ALLOCATOR>& x) {
|
||||
// This if statement works around a bug in boost pool allocator and/or
|
||||
// STL vector where if the size is zero, the pool allocator will allocate
|
||||
// huge amounts of memory.
|
||||
if (x.size() > 0)
|
||||
Base::assign(x.begin(), x.end());
|
||||
}
|
||||
|
||||
/** Conversion to a standard STL container */
|
||||
operator std::vector<VALUE>() const {
|
||||
return std::vector<VALUE>(this->begin(), this->end());
|
||||
}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -22,8 +22,7 @@ using namespace gtsam;
|
|||
/* ************************************************************************* */
|
||||
TEST( testFastContainers, KeySet ) {
|
||||
|
||||
KeyVector init_vector;
|
||||
init_vector += 2, 3, 4, 5;
|
||||
KeyVector init_vector {2, 3, 4, 5};
|
||||
|
||||
KeySet actSet(init_vector);
|
||||
KeySet expSet; expSet += 2, 3, 4, 5;
|
||||
|
|
|
|||
|
|
@ -40,8 +40,8 @@ struct TestNode {
|
|||
struct TestForest {
|
||||
typedef TestNode Node;
|
||||
typedef Node::shared_ptr sharedNode;
|
||||
vector<sharedNode> roots_;
|
||||
const vector<sharedNode>& roots() const { return roots_; }
|
||||
FastVector<sharedNode> roots_;
|
||||
const FastVector<sharedNode>& roots() const { return roots_; }
|
||||
};
|
||||
|
||||
TestForest makeTestForest() {
|
||||
|
|
|
|||
|
|
@ -72,7 +72,7 @@ public:
|
|||
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
|
||||
|
||||
/** A map from keys to values */
|
||||
typedef std::vector<Key> Indices;
|
||||
typedef KeyVector Indices;
|
||||
typedef Assignment<Key> Values;
|
||||
typedef boost::shared_ptr<Values> sharedValues;
|
||||
|
||||
|
|
|
|||
|
|
@ -31,8 +31,8 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
|
||||
vector<Key> DiscreteKeys::indices() const {
|
||||
vector < Key > js;
|
||||
KeyVector DiscreteKeys::indices() const {
|
||||
KeyVector js;
|
||||
for(const DiscreteKey& key: *this)
|
||||
js.push_back(key.first);
|
||||
return js;
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
|
@ -53,7 +54,7 @@ namespace gtsam {
|
|||
GTSAM_EXPORT DiscreteKeys(const std::vector<int>& cs);
|
||||
|
||||
/// Return a vector of indices
|
||||
GTSAM_EXPORT std::vector<Key> indices() const;
|
||||
GTSAM_EXPORT KeyVector indices() const;
|
||||
|
||||
/// Return a map from index to cardinality
|
||||
GTSAM_EXPORT std::map<Key,size_t> cardinalities() const;
|
||||
|
|
|
|||
|
|
@ -130,8 +130,8 @@ namespace gtsam {
|
|||
return keys;
|
||||
}
|
||||
|
||||
vector<Key> Signature::indices() const {
|
||||
vector<Key> js;
|
||||
KeyVector Signature::indices() const {
|
||||
KeyVector js;
|
||||
js.push_back(key_.first);
|
||||
for(const DiscreteKey& key: parents_)
|
||||
js.push_back(key.first);
|
||||
|
|
|
|||
|
|
@ -90,7 +90,7 @@ namespace gtsam {
|
|||
DiscreteKeys discreteKeysParentsFirst() const;
|
||||
|
||||
/** All key indices, with variable key first */
|
||||
std::vector<Key> indices() const;
|
||||
KeyVector indices() const;
|
||||
|
||||
// the CPT as parsed, if successful
|
||||
const boost::optional<Table>& table() const {
|
||||
|
|
|
|||
|
|
@ -67,6 +67,12 @@ public:
|
|||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// Return bearing measurement
|
||||
const B& bearing() const { return bearing_; }
|
||||
|
||||
/// Return range measurement
|
||||
const R& range() const { return range_; }
|
||||
|
||||
/// Prediction function that stacks measurements
|
||||
static BearingRange Measure(
|
||||
const A1& a1, const A2& a2,
|
||||
|
|
|
|||
|
|
@ -19,10 +19,11 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
|
||||
#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/SymmetricBlockMatrix.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -244,7 +245,7 @@ public:
|
|||
template<int N> // N = 2 or 3
|
||||
static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
|
||||
const Eigen::Matrix<double, N, N>& P, const Vector& b,
|
||||
const FastVector<Key>& allKeys, const FastVector<Key>& keys,
|
||||
const KeyVector& allKeys, const KeyVector& keys,
|
||||
/*output ->*/SymmetricBlockMatrix& augmentedHessian) {
|
||||
|
||||
assert(keys.size()==Fs.size());
|
||||
|
|
|
|||
|
|
@ -93,18 +93,12 @@ TEST(CameraSet, Pinhole) {
|
|||
EXPECT(assert_equal(schur, actualReduced.selfadjointView()));
|
||||
|
||||
// Check Schur complement update, same order, should just double
|
||||
FastVector<Key> allKeys, keys;
|
||||
allKeys.push_back(1);
|
||||
allKeys.push_back(2);
|
||||
keys.push_back(1);
|
||||
keys.push_back(2);
|
||||
KeyVector allKeys {1, 2}, keys {1, 2};
|
||||
Set::UpdateSchurComplement<3>(Fs, E, P, b, allKeys, keys, actualReduced);
|
||||
EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.selfadjointView()));
|
||||
|
||||
// Check Schur complement update, keys reversed
|
||||
FastVector<Key> keys2;
|
||||
keys2.push_back(2);
|
||||
keys2.push_back(1);
|
||||
KeyVector keys2 {2, 1};
|
||||
Set::UpdateSchurComplement<3>(Fs, E, P, b, allKeys, keys2, actualReduced);
|
||||
Vector4 reverse_b;
|
||||
reverse_b << b.tail<2>(), b.head<2>();
|
||||
|
|
|
|||
|
|
@ -344,7 +344,7 @@ namespace gtsam {
|
|||
// Get the set of variables to eliminate, which is C1\B.
|
||||
gttic(Full_root_factoring);
|
||||
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
|
||||
FastVector<Key> C1_minus_B; {
|
||||
KeyVector C1_minus_B; {
|
||||
KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
|
||||
for(const Key j: *B->conditional()) {
|
||||
C1_minus_B_set.erase(j); }
|
||||
|
|
@ -356,7 +356,7 @@ namespace gtsam {
|
|||
FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
|
||||
}
|
||||
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
|
||||
FastVector<Key> C2_minus_B; {
|
||||
KeyVector C2_minus_B; {
|
||||
KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
|
||||
for(const Key j: *B->conditional()) {
|
||||
C2_minus_B_set.erase(j); }
|
||||
|
|
@ -460,7 +460,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans)
|
||||
void BayesTree<CLIQUE>::removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans)
|
||||
{
|
||||
// process each key of the new factor
|
||||
for(const Key& j: keys)
|
||||
|
|
|
|||
|
|
@ -214,7 +214,7 @@ namespace gtsam {
|
|||
* Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
|
||||
* Factors and orphans are added to the in/out arguments.
|
||||
*/
|
||||
void removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans);
|
||||
void removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans);
|
||||
|
||||
/**
|
||||
* Remove the requested subtree. */
|
||||
|
|
|
|||
|
|
@ -40,12 +40,12 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
FastVector<Key>
|
||||
KeyVector
|
||||
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
|
||||
{
|
||||
KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
|
||||
KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
|
||||
FastVector<Key> S_setminus_B;
|
||||
KeyVector S_setminus_B;
|
||||
std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
|
||||
indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
|
||||
return S_setminus_B;
|
||||
|
|
@ -53,14 +53,14 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class DERIVED, class FACTORGRAPH>
|
||||
FastVector<Key> BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
|
||||
KeyVector BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
|
||||
const derived_ptr& B, const FactorGraphType& p_Cp_B) const
|
||||
{
|
||||
gttic(shortcut_indices);
|
||||
KeySet allKeys = p_Cp_B.keys();
|
||||
KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
|
||||
FastVector<Key> S_setminus_B = separator_setminus_B(B);
|
||||
FastVector<Key> keep;
|
||||
KeyVector S_setminus_B = separator_setminus_B(B);
|
||||
KeyVector keep;
|
||||
// keep = S\B intersect allKeys (S_setminus_B is already sorted)
|
||||
std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
|
||||
allKeys.begin(), allKeys.end(), back_inserter(keep));
|
||||
|
|
@ -113,7 +113,7 @@ namespace gtsam {
|
|||
gttic(BayesTreeCliqueBase_shortcut);
|
||||
// We only calculate the shortcut when this clique is not B
|
||||
// and when the S\B is not empty
|
||||
FastVector<Key> S_setminus_B = separator_setminus_B(B);
|
||||
KeyVector S_setminus_B = separator_setminus_B(B);
|
||||
if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty())
|
||||
{
|
||||
// Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
|
||||
|
|
@ -124,7 +124,7 @@ namespace gtsam {
|
|||
p_Cp_B += parent->conditional_; // P(Fp|Sp)
|
||||
|
||||
// Determine the variables we want to keepSet, S union B
|
||||
FastVector<Key> keep = shortcut_indices(B, p_Cp_B);
|
||||
KeyVector keep = shortcut_indices(B, p_Cp_B);
|
||||
|
||||
// Marginalize out everything except S union B
|
||||
boost::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function);
|
||||
|
|
@ -170,7 +170,7 @@ namespace gtsam {
|
|||
p_Cp += parent->conditional_; // P(Fp|Sp)
|
||||
|
||||
// The variables we want to keepSet are exactly the ones in S
|
||||
FastVector<Key> indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
|
||||
KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
|
||||
cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -149,12 +149,12 @@ namespace gtsam {
|
|||
protected:
|
||||
|
||||
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations
|
||||
FastVector<Key> separator_setminus_B(const derived_ptr& B) const;
|
||||
KeyVector separator_setminus_B(const derived_ptr& B) const;
|
||||
|
||||
/** Determine variable indices to keep in recursive separator shortcut calculation The factor
|
||||
* graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables
|
||||
* not in S union B. */
|
||||
FastVector<Key> shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
|
||||
KeyVector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
|
||||
|
||||
/** Non-recursive delete cached shortcuts and marginals - internal only. */
|
||||
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
|
||||
|
|
|
|||
|
|
@ -129,7 +129,7 @@ namespace gtsam {
|
|||
template<class FACTORGRAPH>
|
||||
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
|
||||
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
|
||||
const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(variableIndex) {
|
||||
gttic(eliminatePartialSequential);
|
||||
|
|
@ -169,7 +169,7 @@ namespace gtsam {
|
|||
template<class FACTORGRAPH>
|
||||
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
|
||||
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
|
||||
const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(variableIndex) {
|
||||
gttic(eliminatePartialMultifrontal);
|
||||
|
|
@ -190,7 +190,7 @@ namespace gtsam {
|
|||
template<class FACTORGRAPH>
|
||||
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
|
||||
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
|
||||
boost::variant<const Ordering&, const std::vector<Key>&> variables,
|
||||
boost::variant<const Ordering&, const KeyVector&> variables,
|
||||
OptionalOrdering marginalizedVariableOrdering,
|
||||
const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
|
|
@ -223,9 +223,9 @@ namespace gtsam {
|
|||
// No ordering was provided for the marginalized variables, so order them using constrained
|
||||
// COLAMD.
|
||||
bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
|
||||
const std::vector<Key>* variablesOrOrdering =
|
||||
const KeyVector* variablesOrOrdering =
|
||||
unmarginalizedAreOrdered ?
|
||||
boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
|
||||
boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
|
||||
|
||||
Ordering totalOrdering =
|
||||
Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
|
||||
|
|
@ -249,7 +249,7 @@ namespace gtsam {
|
|||
template<class FACTORGRAPH>
|
||||
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
|
||||
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
|
||||
boost::variant<const Ordering&, const std::vector<Key>&> variables,
|
||||
boost::variant<const Ordering&, const KeyVector&> variables,
|
||||
OptionalOrdering marginalizedVariableOrdering,
|
||||
const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
|
|
@ -282,9 +282,9 @@ namespace gtsam {
|
|||
// No ordering was provided for the marginalized variables, so order them using constrained
|
||||
// COLAMD.
|
||||
bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
|
||||
const std::vector<Key>* variablesOrOrdering =
|
||||
const KeyVector* variablesOrOrdering =
|
||||
unmarginalizedAreOrdered ?
|
||||
boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
|
||||
boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
|
||||
|
||||
Ordering totalOrdering =
|
||||
Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
|
||||
|
|
@ -308,7 +308,7 @@ namespace gtsam {
|
|||
template<class FACTORGRAPH>
|
||||
boost::shared_ptr<FACTORGRAPH>
|
||||
EliminateableFactorGraph<FACTORGRAPH>::marginal(
|
||||
const std::vector<Key>& variables,
|
||||
const KeyVector& variables,
|
||||
const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(variableIndex)
|
||||
|
|
|
|||
|
|
@ -170,7 +170,7 @@ namespace gtsam {
|
|||
* factor graph, and \f$ B = X\backslash A \f$. */
|
||||
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
|
||||
eliminatePartialSequential(
|
||||
const std::vector<Key>& variables,
|
||||
const KeyVector& variables,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
|
|
@ -190,14 +190,14 @@ namespace gtsam {
|
|||
* factor graph, and \f$ B = X\backslash A \f$. */
|
||||
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
|
||||
eliminatePartialMultifrontal(
|
||||
const std::vector<Key>& variables,
|
||||
const KeyVector& variables,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
/** Compute the marginal of the requested variables and return the result as a Bayes net.
|
||||
* @param variables Determines the variables whose marginal to compute, if provided as an
|
||||
* Ordering they will be ordered in the returned BayesNet as specified, and if provided
|
||||
* as a vector<Key> they will be ordered using constrained COLAMD.
|
||||
* as a KeyVector they will be ordered using constrained COLAMD.
|
||||
* @param marginalizedVariableOrdering Optional ordering for the variables being marginalized
|
||||
* out, i.e. all variables not in \c variables. If this is boost::none, the ordering
|
||||
* will be computed with COLAMD.
|
||||
|
|
@ -206,7 +206,7 @@ namespace gtsam {
|
|||
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
|
||||
* provided one will be computed. */
|
||||
boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
|
||||
boost::variant<const Ordering&, const std::vector<Key>&> variables,
|
||||
boost::variant<const Ordering&, const KeyVector&> variables,
|
||||
OptionalOrdering marginalizedVariableOrdering = boost::none,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
|
@ -214,7 +214,7 @@ namespace gtsam {
|
|||
/** Compute the marginal of the requested variables and return the result as a Bayes tree.
|
||||
* @param variables Determines the variables whose marginal to compute, if provided as an
|
||||
* Ordering they will be ordered in the returned BayesNet as specified, and if provided
|
||||
* as a vector<Key> they will be ordered using constrained COLAMD.
|
||||
* as a KeyVector they will be ordered using constrained COLAMD.
|
||||
* @param marginalizedVariableOrdering Optional ordering for the variables being marginalized
|
||||
* out, i.e. all variables not in \c variables. If this is boost::none, the ordering
|
||||
* will be computed with COLAMD.
|
||||
|
|
@ -223,14 +223,14 @@ namespace gtsam {
|
|||
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
|
||||
* provided one will be computed. */
|
||||
boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
|
||||
boost::variant<const Ordering&, const std::vector<Key>&> variables,
|
||||
boost::variant<const Ordering&, const KeyVector&> variables,
|
||||
OptionalOrdering marginalizedVariableOrdering = boost::none,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
/** Compute the marginal factor graph of the requested variables. */
|
||||
boost::shared_ptr<FactorGraphType> marginal(
|
||||
const std::vector<Key>& variables,
|
||||
const KeyVector& variables,
|
||||
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
|
||||
OptionalVariableIndex variableIndex = boost::none) const;
|
||||
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@ namespace gtsam {
|
|||
gatheredFactors.push_back(childrenResults.begin(), childrenResults.end());
|
||||
|
||||
// Do dense elimination step
|
||||
FastVector<Key> keyAsVector(1); keyAsVector[0] = key;
|
||||
KeyVector keyAsVector(1); keyAsVector[0] = key;
|
||||
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
|
||||
function(gatheredFactors, Ordering(keyAsVector));
|
||||
|
||||
|
|
|
|||
|
|
@ -58,15 +58,15 @@ namespace gtsam {
|
|||
|
||||
public:
|
||||
/// Iterator over keys
|
||||
typedef FastVector<Key>::iterator iterator;
|
||||
typedef KeyVector::iterator iterator;
|
||||
|
||||
/// Const iterator over keys
|
||||
typedef FastVector<Key>::const_iterator const_iterator;
|
||||
typedef KeyVector::const_iterator const_iterator;
|
||||
|
||||
protected:
|
||||
|
||||
/// The keys involved in this factor
|
||||
FastVector<Key> keys_;
|
||||
KeyVector keys_;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
|
@ -112,7 +112,7 @@ namespace gtsam {
|
|||
const_iterator find(Key key) const { return std::find(begin(), end(), key); }
|
||||
|
||||
/// Access the factor's involved variable keys
|
||||
const FastVector<Key>& keys() const { return keys_; }
|
||||
const KeyVector& keys() const { return keys_; }
|
||||
|
||||
/** Iterator at beginning of involved variable keys */
|
||||
const_iterator begin() const { return keys_.begin(); }
|
||||
|
|
@ -148,7 +148,7 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/** @return keys involved in this factor */
|
||||
FastVector<Key>& keys() { return keys_; }
|
||||
KeyVector& keys() { return keys_; }
|
||||
|
||||
/** Iterator at beginning of involved variable keys */
|
||||
iterator begin() { return keys_.begin(); }
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@ void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
|
|||
BayesNetType bn;
|
||||
const KeySet newFactorKeys = newFactors.keys();
|
||||
if (!this->empty()) {
|
||||
std::vector<Key> keyVector(newFactorKeys.begin(), newFactorKeys.end());
|
||||
KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
|
||||
this->removeTop(keyVector, bn, orphans);
|
||||
}
|
||||
|
||||
|
|
@ -46,7 +46,7 @@ void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
|
|||
// Get an ordering where the new keys are eliminated last
|
||||
const VariableIndex index(factors);
|
||||
const Ordering ordering = Ordering::ColamdConstrainedLast(index,
|
||||
std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
|
||||
KeyVector(newFactorKeys.begin(), newFactorKeys.end()));
|
||||
|
||||
// eliminate all factors (top, added, orphans) into a new Bayes tree
|
||||
auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
|
|||
static const gtsam::KeyFormatter MultiRobotKeyFormatter =
|
||||
&_multirobotKeyFormatter;
|
||||
|
||||
/// Useful typedef for operations with Values - allows for matlab interface
|
||||
/// Define collection type once and for all - also used in wrappers
|
||||
typedef FastVector<Key> KeyVector;
|
||||
|
||||
// TODO(frank): Nothing fast about these :-(
|
||||
|
|
|
|||
|
|
@ -19,7 +19,6 @@
|
|||
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/base/FastVector.h>
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
|
||||
|
|
@ -49,9 +48,8 @@ public:
|
|||
typedef boost::bimap<Key, int32_t> bm_type;
|
||||
|
||||
private:
|
||||
FastVector<int32_t> xadj_; // Index of node's adjacency list in adj
|
||||
FastVector<int32_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector
|
||||
FastVector<int32_t> iadj_; // Integer keys for passing into metis. One to one mapping with adj_;
|
||||
std::vector<int32_t> xadj_; // Index of node's adjacency list in adj
|
||||
std::vector<int32_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector
|
||||
boost::bimap<Key, int32_t> intKeyBMap_; // Stores Key <-> integer value relationship
|
||||
size_t nKeys_;
|
||||
|
||||
|
|
@ -83,10 +81,10 @@ public:
|
|||
template<class FACTOR>
|
||||
void augment(const FactorGraph<FACTOR>& factors);
|
||||
|
||||
std::vector<int32_t> xadj() const {
|
||||
const std::vector<int32_t>& xadj() const {
|
||||
return xadj_;
|
||||
}
|
||||
std::vector<int32_t> adj() const {
|
||||
const std::vector<int32_t>& adj() const {
|
||||
return adj_;
|
||||
}
|
||||
size_t nValues() const {
|
||||
|
|
|
|||
|
|
@ -61,7 +61,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
|
|||
|
||||
if (nVars == 1)
|
||||
{
|
||||
return Ordering(std::vector<Key>(1, variableIndex.begin()->first));
|
||||
return Ordering(KeyVector(1, variableIndex.begin()->first));
|
||||
}
|
||||
|
||||
const size_t nEntries = variableIndex.nEntries(), nFactors =
|
||||
|
|
@ -75,7 +75,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
|
|||
// Fill in input data for COLAMD
|
||||
p[0] = 0;
|
||||
int count = 0;
|
||||
vector<Key> keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
|
||||
KeyVector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
|
||||
size_t index = 0;
|
||||
for (auto key_factors: variableIndex) {
|
||||
// Arrange factor indices into COLAMD format
|
||||
|
|
@ -127,7 +127,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
|
|||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
|
||||
const std::vector<Key>& constrainLast, bool forceOrder) {
|
||||
const KeyVector& constrainLast, bool forceOrder) {
|
||||
gttic(Ordering_COLAMDConstrainedLast);
|
||||
|
||||
size_t n = variableIndex.size();
|
||||
|
|
@ -154,7 +154,7 @@ Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
|
|||
|
||||
/* ************************************************************************* */
|
||||
Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex,
|
||||
const std::vector<Key>& constrainFirst, bool forceOrder) {
|
||||
const KeyVector& constrainFirst, bool forceOrder) {
|
||||
gttic(Ordering_COLAMDConstrainedFirst);
|
||||
|
||||
const int none = -1;
|
||||
|
|
|
|||
|
|
@ -31,9 +31,9 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class Ordering: public std::vector<Key> {
|
||||
class Ordering: public KeyVector {
|
||||
protected:
|
||||
typedef std::vector<Key> Base;
|
||||
typedef KeyVector Base;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -94,12 +94,12 @@ public:
|
|||
/// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains
|
||||
/// the variables in \c constrainLast to the end of the ordering, and orders all other variables
|
||||
/// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c
|
||||
/// constrainLast will be ordered in the same order specified in the vector<Key> \c
|
||||
/// constrainLast will be ordered in the same order specified in the KeyVector \c
|
||||
/// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
|
||||
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
|
||||
template<class FACTOR_GRAPH>
|
||||
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH& graph,
|
||||
const std::vector<Key>& constrainLast, bool forceOrder = false) {
|
||||
const KeyVector& constrainLast, bool forceOrder = false) {
|
||||
if (graph.empty())
|
||||
return Ordering();
|
||||
else
|
||||
|
|
@ -109,11 +109,11 @@ public:
|
|||
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This
|
||||
/// function constrains the variables in \c constrainLast to the end of the ordering, and orders
|
||||
/// all other variables before in a fill-reducing ordering. If \c forceOrder is true, the
|
||||
/// variables in \c constrainLast will be ordered in the same order specified in the vector<Key>
|
||||
/// variables in \c constrainLast will be ordered in the same order specified in the KeyVector
|
||||
/// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
|
||||
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
|
||||
static GTSAM_EXPORT Ordering ColamdConstrainedLast(
|
||||
const VariableIndex& variableIndex, const std::vector<Key>& constrainLast,
|
||||
const VariableIndex& variableIndex, const KeyVector& constrainLast,
|
||||
bool forceOrder = false);
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
|
||||
|
|
@ -121,12 +121,12 @@ public:
|
|||
/// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains
|
||||
/// the variables in \c constrainLast to the end of the ordering, and orders all other variables
|
||||
/// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c
|
||||
/// constrainFirst will be ordered in the same order specified in the vector<Key> \c
|
||||
/// constrainFirst will be ordered in the same order specified in the KeyVector \c
|
||||
/// constrainFirst. If \c forceOrder is false, the variables in \c constrainFirst will be
|
||||
/// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
|
||||
template<class FACTOR_GRAPH>
|
||||
static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH& graph,
|
||||
const std::vector<Key>& constrainFirst, bool forceOrder = false) {
|
||||
const KeyVector& constrainFirst, bool forceOrder = false) {
|
||||
if (graph.empty())
|
||||
return Ordering();
|
||||
else
|
||||
|
|
@ -137,12 +137,12 @@ public:
|
|||
/// function constrains the variables in \c constrainFirst to the front of the ordering, and
|
||||
/// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the
|
||||
/// variables in \c constrainFirst will be ordered in the same order specified in the
|
||||
/// vector<Key> \c constrainFirst. If \c forceOrder is false, the variables in \c
|
||||
/// KeyVector \c constrainFirst. If \c forceOrder is false, the variables in \c
|
||||
/// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to
|
||||
/// reduce fill-in as well.
|
||||
static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
|
||||
const VariableIndex& variableIndex,
|
||||
const std::vector<Key>& constrainFirst, bool forceOrder = false);
|
||||
const KeyVector& constrainFirst, bool forceOrder = false);
|
||||
|
||||
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
|
||||
/// for note on performance). This internally builds a VariableIndex so if you already have a
|
||||
|
|
@ -176,7 +176,7 @@ public:
|
|||
template<class FACTOR_GRAPH>
|
||||
static Ordering Natural(const FACTOR_GRAPH &fg) {
|
||||
KeySet src = fg.keys();
|
||||
std::vector<Key> keys(src.begin(), src.end());
|
||||
KeyVector keys(src.begin(), src.end());
|
||||
std::stable_sort(keys.begin(), keys.end());
|
||||
return Ordering(keys);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@ struct BinaryJacobianFactor: JacobianFactor {
|
|||
}
|
||||
|
||||
// Fixed-size matrix update
|
||||
void updateHessian(const FastVector<Key>& infoKeys,
|
||||
void updateHessian(const KeyVector& infoKeys,
|
||||
SymmetricBlockMatrix* info) const {
|
||||
gttic(updateHessian_BinaryJacobianFactor);
|
||||
// Whiten the factor if it has a noise model
|
||||
|
|
|
|||
|
|
@ -117,7 +117,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
VectorValues GaussianConditional::solve(const VectorValues& x) const {
|
||||
// Concatenate all vector values that correspond to parent variables
|
||||
const Vector xS = x.vector(FastVector<Key>(beginParents(), endParents()));
|
||||
const Vector xS = x.vector(KeyVector(beginParents(), endParents()));
|
||||
|
||||
// Update right-hand-side
|
||||
const Vector rhs = get_d() - get_S() * xS;
|
||||
|
|
@ -145,10 +145,10 @@ namespace gtsam {
|
|||
VectorValues GaussianConditional::solveOtherRHS(
|
||||
const VectorValues& parents, const VectorValues& rhs) const {
|
||||
// Concatenate all vector values that correspond to parent variables
|
||||
Vector xS = parents.vector(FastVector<Key>(beginParents(), endParents()));
|
||||
Vector xS = parents.vector(KeyVector(beginParents(), endParents()));
|
||||
|
||||
// Instead of updating getb(), update the right-hand-side from the given rhs
|
||||
const Vector rhsR = rhs.vector(FastVector<Key>(beginFrontals(), endFrontals()));
|
||||
const Vector rhsR = rhs.vector(KeyVector(beginFrontals(), endFrontals()));
|
||||
xS = rhsR - get_S() * xS;
|
||||
|
||||
// Solve Matrix
|
||||
|
|
@ -171,7 +171,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
|
||||
Vector frontalVec = gy.vector(FastVector<Key>(beginFrontals(), endFrontals()));
|
||||
Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
|
||||
frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
|
||||
|
||||
// Check for indeterminant solution
|
||||
|
|
|
|||
|
|
@ -126,7 +126,7 @@ namespace gtsam {
|
|||
* @param scatter A mapping from variable index to slot index in this HessianFactor
|
||||
* @param info The information matrix to be updated
|
||||
*/
|
||||
virtual void updateHessian(const FastVector<Key>& keys,
|
||||
virtual void updateHessian(const KeyVector& keys,
|
||||
SymmetricBlockMatrix* info) const = 0;
|
||||
|
||||
/// y += alpha * A'*A*x
|
||||
|
|
|
|||
|
|
@ -155,7 +155,7 @@ DenseIndex _getSizeHF(const Vector& m) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(const std::vector<Key>& js,
|
||||
HessianFactor::HessianFactor(const KeyVector& js,
|
||||
const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
|
||||
GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) {
|
||||
// Get the number of variables
|
||||
|
|
@ -356,7 +356,7 @@ double HessianFactor::error(const VectorValues& c) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
|
||||
void HessianFactor::updateHessian(const KeyVector& infoKeys,
|
||||
SymmetricBlockMatrix* info) const {
|
||||
gttic(updateHessian_HessianFactor);
|
||||
assert(info);
|
||||
|
|
|
|||
|
|
@ -159,7 +159,7 @@ namespace gtsam {
|
|||
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces
|
||||
* of the linear vector term, and f the constant term.
|
||||
*/
|
||||
HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
|
||||
HessianFactor(const KeyVector& js, const std::vector<Matrix>& Gs,
|
||||
const std::vector<Vector>& gs, double f);
|
||||
|
||||
/** Constructor with an arbitrary number of keys and with the augmented information matrix
|
||||
|
|
@ -310,7 +310,7 @@ namespace gtsam {
|
|||
* @param keys THe ordered vector of keys for the information matrix to be updated
|
||||
* @param info The information matrix to be updated
|
||||
*/
|
||||
void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const;
|
||||
void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const;
|
||||
|
||||
/** Update another Hessian factor
|
||||
* @param other the HessianFactor to be updated
|
||||
|
|
|
|||
|
|
@ -501,7 +501,7 @@ map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void JacobianFactor::updateHessian(const FastVector<Key>& infoKeys,
|
||||
void JacobianFactor::updateHessian(const KeyVector& infoKeys,
|
||||
SymmetricBlockMatrix* info) const {
|
||||
gttic(updateHessian_JacobianFactor);
|
||||
|
||||
|
|
|
|||
|
|
@ -283,7 +283,7 @@ namespace gtsam {
|
|||
* @param scatter A mapping from variable index to slot index in this HessianFactor
|
||||
* @param info The information matrix to be updated
|
||||
*/
|
||||
void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const;
|
||||
void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const;
|
||||
|
||||
/** Return A*x */
|
||||
Vector operator*(const VectorValues& x) const;
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ public:
|
|||
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces
|
||||
* of the linear vector term, and f the constant term.
|
||||
*/
|
||||
RegularHessianFactor(const std::vector<Key>& js,
|
||||
RegularHessianFactor(const KeyVector& js,
|
||||
const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
|
||||
HessianFactor(js, Gs, gs, f) {
|
||||
checkInvariants();
|
||||
|
|
|
|||
|
|
@ -571,14 +571,14 @@ void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
|
|||
/* in place back substitute */
|
||||
for (auto cg: boost::adaptors::reverse(*Rc1_)) {
|
||||
/* collect a subvector of x that consists of the parents of cg (S) */
|
||||
const Vector xParent = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginParents(), cg->endParents()));
|
||||
const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
|
||||
const Vector xParent = getSubvector(x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents()));
|
||||
const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
|
||||
|
||||
/* compute the solution for the current pivot */
|
||||
const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);
|
||||
|
||||
/* assign subvector of sol to the frontal variables */
|
||||
setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
|
||||
setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -590,7 +590,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
|
|||
|
||||
/* in place back substitute */
|
||||
for(const boost::shared_ptr<GaussianConditional> & cg: *Rc1_) {
|
||||
const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
|
||||
const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
|
||||
// const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
|
||||
const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
|
||||
|
||||
|
|
@ -598,7 +598,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
|
|||
if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front());
|
||||
|
||||
/* assign subvector of sol to the frontal variables */
|
||||
setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
|
||||
setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
|
||||
|
||||
/* substract from parent variables */
|
||||
for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) {
|
||||
|
|
@ -626,7 +626,7 @@ void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo
|
|||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys) {
|
||||
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) {
|
||||
|
||||
/* a cache of starting index and dim */
|
||||
typedef vector<pair<size_t, size_t> > Cache;
|
||||
|
|
@ -652,7 +652,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<
|
|||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst) {
|
||||
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) {
|
||||
/* use the cache */
|
||||
size_t idx = 0;
|
||||
for ( const Key &key: keys ) {
|
||||
|
|
|
|||
|
|
@ -298,10 +298,10 @@ namespace gtsam {
|
|||
};
|
||||
|
||||
/* get subvectors */
|
||||
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys);
|
||||
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys);
|
||||
|
||||
/* set subvectors */
|
||||
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst);
|
||||
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst);
|
||||
|
||||
|
||||
/* build a factor subgraph, which is defined as a set of weighted edges (factors) */
|
||||
|
|
|
|||
|
|
@ -229,7 +229,7 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
|
|||
VectorValues actual = gbn.optimizeGradientSearch();
|
||||
|
||||
// Check that points agree
|
||||
FastVector<Key> keys = list_of(0)(1)(2)(3)(4);
|
||||
KeyVector keys {0, 1, 2, 3, 4};
|
||||
Vector actualAsVector = actual.vector(keys);
|
||||
EXPECT(assert_equal(expected, actualAsVector, 1e-5));
|
||||
|
||||
|
|
|
|||
|
|
@ -287,7 +287,7 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
|
|||
VectorValues actual = bt.optimizeGradientSearch();
|
||||
|
||||
// Check that points agree
|
||||
FastVector<Key> keys = list_of(0)(1)(2)(3)(4);
|
||||
KeyVector keys {0, 1, 2, 3, 4};
|
||||
EXPECT(assert_equal(expected, actual.vector(keys), 1e-5));
|
||||
EXPECT(assert_equal(expectedFromBN, actual, 1e-5));
|
||||
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@ const double tol = 1e-5;
|
|||
/* ************************************************************************* */
|
||||
TEST(HessianFactor, Slot)
|
||||
{
|
||||
FastVector<Key> keys = list_of(2)(4)(1);
|
||||
KeyVector keys {2, 4, 1};
|
||||
EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2));
|
||||
EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4));
|
||||
EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1));
|
||||
|
|
@ -252,8 +252,7 @@ TEST(HessianFactor, ConstructorNWay)
|
|||
(1, dx1)
|
||||
(2, dx2);
|
||||
|
||||
std::vector<Key> js;
|
||||
js.push_back(0); js.push_back(1); js.push_back(2);
|
||||
KeyVector js {0, 1, 2};
|
||||
std::vector<Matrix> Gs;
|
||||
Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
|
||||
std::vector<Vector> gs;
|
||||
|
|
@ -517,7 +516,7 @@ TEST(HessianFactor, gradientAtZero)
|
|||
// test gradient at zero
|
||||
VectorValues expectedG = pair_list_of<Key, Vector>(0, -g1) (1, -g2);
|
||||
Matrix A; Vector b; boost::tie(A,b) = factor.jacobian();
|
||||
FastVector<Key> keys; keys += 0,1;
|
||||
KeyVector keys {0, 1};
|
||||
EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys)));
|
||||
VectorValues actualG = factor.gradientAtZero();
|
||||
EXPECT(assert_equal(expectedG, actualG));
|
||||
|
|
|
|||
|
|
@ -350,7 +350,7 @@ TEST(JacobianFactor, operators )
|
|||
VectorValues expectedG;
|
||||
expectedG.insert(1, Vector2(20,-10));
|
||||
expectedG.insert(2, Vector2(-20, 10));
|
||||
FastVector<Key> keys; keys += 1,2;
|
||||
KeyVector keys {1, 2};
|
||||
EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys)));
|
||||
VectorValues actualG = lf.gradientAtZero();
|
||||
EXPECT(assert_equal(expectedG, actualG));
|
||||
|
|
|
|||
|
|
@ -64,7 +64,7 @@ TEST(RegularHessianFactor, Constructors)
|
|||
EXPECT(assert_equal(factor,factor2));
|
||||
|
||||
// Test n-way constructor
|
||||
vector<Key> keys; keys += 0, 1, 3;
|
||||
KeyVector keys {0, 1, 3};
|
||||
vector<Matrix> Gs; Gs += G11, G12, G13, G22, G23, G33;
|
||||
vector<Vector> gs; gs += g1, g2, g3;
|
||||
RegularHessianFactor<2> factor3(keys, Gs, gs, f);
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ TEST(VectorValues, basics)
|
|||
EXPECT(assert_equal(Vector2(2, 3), actual[1]));
|
||||
EXPECT(assert_equal(Vector2(4, 5), actual[2]));
|
||||
EXPECT(assert_equal(Vector2(6, 7), actual[5]));
|
||||
FastVector<Key> keys = list_of(0)(1)(2)(5);
|
||||
KeyVector keys {0, 1, 2, 5};
|
||||
EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), actual.vector(keys)));
|
||||
|
||||
// Check exceptions
|
||||
|
|
@ -101,8 +101,7 @@ TEST(VectorValues, subvector)
|
|||
init.insert(12, Vector2(4, 5));
|
||||
init.insert(13, Vector2(6, 7));
|
||||
|
||||
std::vector<Key> keys;
|
||||
keys += 10, 12, 13;
|
||||
KeyVector keys {10, 12, 13};
|
||||
Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished();
|
||||
EXPECT(assert_equal(expSubVector, init.vector(keys)));
|
||||
}
|
||||
|
|
@ -197,7 +196,7 @@ TEST(VectorValues, convert)
|
|||
EXPECT(assert_equal(expected, actual2));
|
||||
|
||||
// Test other direction, note vector() is not guaranteed to give right result
|
||||
FastVector<Key> keys = list_of(0)(1)(2)(5);
|
||||
KeyVector keys {0, 1, 2, 5};
|
||||
EXPECT(assert_equal(x, actual.vector(keys)));
|
||||
|
||||
// Test version with dims argument
|
||||
|
|
@ -222,7 +221,7 @@ TEST(VectorValues, vector_sub)
|
|||
expected << 1, 6, 7;
|
||||
|
||||
// Test FastVector version
|
||||
FastVector<Key> keys = list_of(0)(5);
|
||||
KeyVector keys {0, 5};
|
||||
EXPECT(assert_equal(expected, vv.vector(keys)));
|
||||
|
||||
// Test version with dims argument
|
||||
|
|
|
|||
|
|
@ -22,19 +22,26 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <GeographicLib/Config.h>
|
||||
#include <GeographicLib/LocalCartesian.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace GeographicLib;
|
||||
|
||||
#if GEOGRAPHICLIB_VERSION_MINOR<37
|
||||
static const auto& kWGS84 = Geocentric::WGS84;
|
||||
#else
|
||||
static const auto& kWGS84 = Geocentric::WGS84();
|
||||
#endif
|
||||
|
||||
// *************************************************************************
|
||||
namespace example {
|
||||
// ENU Origin is where the plane was in hold next to runway
|
||||
const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
|
||||
|
||||
// Convert from GPS to ENU
|
||||
LocalCartesian origin_ENU(lat0, lon0, h0, Geocentric::WGS84);
|
||||
LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);
|
||||
|
||||
// Dekalb-Peachtree Airport runway 2L
|
||||
const double lat = 33.87071, lon = -84.30482, h = 274;
|
||||
|
|
@ -107,8 +114,7 @@ TEST(GPSData, init) {
|
|||
// GPS Reading 1 will be ENU origin
|
||||
double t1 = 84831;
|
||||
Point3 NED1(0, 0, 0);
|
||||
LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54,
|
||||
Geocentric::WGS84);
|
||||
LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54, kWGS84);
|
||||
|
||||
// GPS Readin 2
|
||||
double t2 = 84831.5;
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <GeographicLib/Config.h>
|
||||
#include <GeographicLib/Geocentric.hpp>
|
||||
#include <GeographicLib/UTMUPS.hpp>
|
||||
#include <GeographicLib/LocalCartesian.hpp>
|
||||
|
|
@ -22,6 +23,8 @@
|
|||
#include <gtsam/base/types.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -29,21 +32,27 @@ using namespace std;
|
|||
using namespace GeographicLib;
|
||||
|
||||
// Dekalb-Peachtree Airport runway 2L
|
||||
const double lat = 33.87071, lon = -84.30482, h = 274;
|
||||
static const double lat = 33.87071, lon = -84.30482, h = 274;
|
||||
|
||||
#if GEOGRAPHICLIB_VERSION_MINOR<37
|
||||
static const auto& kWGS84 = Geocentric::WGS84;
|
||||
#else
|
||||
static const auto& kWGS84 = Geocentric::WGS84();
|
||||
#endif
|
||||
|
||||
//**************************************************************************
|
||||
TEST( GeographicLib, Geocentric) {
|
||||
|
||||
// From lat-lon to geocentric
|
||||
double X, Y, Z;
|
||||
Geocentric::WGS84.Forward(lat, lon, h, X, Y, Z);
|
||||
kWGS84.Forward(lat, lon, h, X, Y, Z);
|
||||
EXPECT_DOUBLES_EQUAL(526, X/1000, 1);
|
||||
EXPECT_DOUBLES_EQUAL(-5275, Y/1000, 1);
|
||||
EXPECT_DOUBLES_EQUAL(3535, Z/1000, 1);
|
||||
|
||||
// From geocentric to lat-lon
|
||||
double lat_, lon_, h_;
|
||||
Geocentric::WGS84.Reverse(X, Y, Z, lat_, lon_, h_);
|
||||
kWGS84.Reverse(X, Y, Z, lat_, lon_, h_);
|
||||
EXPECT_DOUBLES_EQUAL(lat, lat_, 1e-5);
|
||||
EXPECT_DOUBLES_EQUAL(lon, lon_, 1e-5);
|
||||
EXPECT_DOUBLES_EQUAL(h, h_, 1e-5);
|
||||
|
|
@ -61,7 +70,9 @@ TEST( GeographicLib, UTM) {
|
|||
// UTM is 16N 749305.58 3751090.08
|
||||
// Obtained by
|
||||
// http://geographiclib.sourceforge.net/cgi-bin/GeoConvert?input=33.87071+-84.30482000000001&zone=-3&prec=2&option=Submit
|
||||
EXPECT(UTMUPS::EncodeZone(zone, northp)=="16N");
|
||||
auto actual = UTMUPS::EncodeZone(zone, northp);
|
||||
boost::to_upper(actual);
|
||||
EXPECT(actual=="16N");
|
||||
EXPECT_DOUBLES_EQUAL(749305.58, x, 1e-2);
|
||||
EXPECT_DOUBLES_EQUAL(3751090.08, y, 1e-2);
|
||||
}
|
||||
|
|
@ -69,11 +80,9 @@ TEST( GeographicLib, UTM) {
|
|||
//**************************************************************************
|
||||
TEST( GeographicLib, ENU) {
|
||||
|
||||
const Geocentric& earth = Geocentric::WGS84;
|
||||
|
||||
// ENU Origin is where the plane was in hold next to runway
|
||||
const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
|
||||
LocalCartesian enu(lat0, lon0, h0, earth);
|
||||
LocalCartesian enu(lat0, lon0, h0, kWGS84);
|
||||
|
||||
// From lat-lon to geocentric
|
||||
double E, N, U;
|
||||
|
|
|
|||
|
|
@ -239,10 +239,10 @@ void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys,
|
|||
// Update the current variable
|
||||
// Get VectorValues slice corresponding to current variables
|
||||
Vector gR =
|
||||
grad.vector(FastVector<Key>(clique->conditional()->beginFrontals(),
|
||||
grad.vector(KeyVector(clique->conditional()->beginFrontals(),
|
||||
clique->conditional()->endFrontals()));
|
||||
Vector gS =
|
||||
grad.vector(FastVector<Key>(clique->conditional()->beginParents(),
|
||||
grad.vector(KeyVector(clique->conditional()->beginParents(),
|
||||
clique->conditional()->endParents()));
|
||||
|
||||
// Compute R*g and S*g for this clique
|
||||
|
|
|
|||
|
|
@ -195,7 +195,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) {
|
|||
/* ************************************************************************* */
|
||||
boost::shared_ptr<KeySet> ISAM2::recalculate(
|
||||
const KeySet& markedKeys, const KeySet& relinKeys,
|
||||
const vector<Key>& observedKeys, const KeySet& unusedIndices,
|
||||
const KeyVector& observedKeys, const KeySet& unusedIndices,
|
||||
const boost::optional<FastMap<Key, int> >& constrainKeys,
|
||||
ISAM2Result* result) {
|
||||
// TODO(dellaert): new factors are linearized twice,
|
||||
|
|
@ -243,7 +243,7 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
|
|||
gttic(removetop);
|
||||
Cliques orphans;
|
||||
GaussianBayesNet affectedBayesNet;
|
||||
this->removeTop(FastVector<Key>(markedKeys.begin(), markedKeys.end()),
|
||||
this->removeTop(KeyVector(markedKeys.begin(), markedKeys.end()),
|
||||
affectedBayesNet, orphans);
|
||||
gttoc(removetop);
|
||||
|
||||
|
|
@ -667,7 +667,7 @@ ISAM2Result ISAM2::update(
|
|||
// NOTE: we use assign instead of the iterator constructor here because this
|
||||
// is a vector of size_t, so the constructor unintentionally resolves to
|
||||
// vector(size_t count, Key value) instead of the iterator constructor.
|
||||
FastVector<Key> observedKeys;
|
||||
KeyVector observedKeys;
|
||||
observedKeys.reserve(markedKeys.size());
|
||||
for (Key index : markedKeys) {
|
||||
if (unusedIndices.find(index) ==
|
||||
|
|
@ -945,7 +945,7 @@ void ISAM2::marginalizeLeaves(
|
|||
// conditional
|
||||
auto cg = clique->conditional();
|
||||
const KeySet cliqueFrontals(cg->beginFrontals(), cg->endFrontals());
|
||||
FastVector<Key> cliqueFrontalsToEliminate;
|
||||
KeyVector cliqueFrontalsToEliminate;
|
||||
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(),
|
||||
leafKeys.begin(), leafKeys.end(),
|
||||
std::back_inserter(cliqueFrontalsToEliminate));
|
||||
|
|
@ -967,7 +967,7 @@ void ISAM2::marginalizeLeaves(
|
|||
cg->matrixObject().rowStart() = dimToRemove;
|
||||
|
||||
// Change the keys in the clique
|
||||
FastVector<Key> originalKeys;
|
||||
KeyVector originalKeys;
|
||||
originalKeys.swap(cg->keys());
|
||||
cg->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end());
|
||||
cg->nrFrontals() -= nToRemove;
|
||||
|
|
|
|||
|
|
@ -299,7 +299,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
|
|||
|
||||
virtual boost::shared_ptr<KeySet> recalculate(
|
||||
const KeySet& markedKeys, const KeySet& relinKeys,
|
||||
const std::vector<Key>& observedKeys, const KeySet& unusedIndices,
|
||||
const KeyVector& observedKeys, const KeySet& unusedIndices,
|
||||
const boost::optional<FastMap<Key, int> >& constrainKeys,
|
||||
ISAM2Result* result);
|
||||
|
||||
|
|
|
|||
|
|
@ -80,14 +80,14 @@ Matrix Marginals::marginalCovariance(Key variable) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JointMarginal Marginals::jointMarginalCovariance(const std::vector<Key>& variables) const {
|
||||
JointMarginal Marginals::jointMarginalCovariance(const KeyVector& variables) const {
|
||||
JointMarginal info = jointMarginalInformation(variables);
|
||||
info.blockMatrix_.invertInPlace();
|
||||
return info;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variables) const {
|
||||
JointMarginal Marginals::jointMarginalInformation(const KeyVector& variables) const {
|
||||
|
||||
// If 2 variables, we can use the BayesTree::joint function, otherwise we
|
||||
// have to use sequential elimination.
|
||||
|
|
@ -119,7 +119,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
|
|||
Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1);
|
||||
|
||||
// Information matrix will be returned with sorted keys
|
||||
std::vector<Key> variablesSorted = variables;
|
||||
KeyVector variablesSorted = variables;
|
||||
std::sort(variablesSorted.begin(), variablesSorted.end());
|
||||
|
||||
// Get dimensions from factor graph
|
||||
|
|
|
|||
|
|
@ -74,10 +74,10 @@ public:
|
|||
Matrix marginalCovariance(Key variable) const;
|
||||
|
||||
/** Compute the joint marginal covariance of several variables */
|
||||
JointMarginal jointMarginalCovariance(const std::vector<Key>& variables) const;
|
||||
JointMarginal jointMarginalCovariance(const KeyVector& variables) const;
|
||||
|
||||
/** Compute the joint marginal information of several variables */
|
||||
JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const;
|
||||
JointMarginal jointMarginalInformation(const KeyVector& variables) const;
|
||||
|
||||
/** Optimize the bayes tree */
|
||||
VectorValues optimize() const;
|
||||
|
|
@ -130,7 +130,7 @@ public:
|
|||
void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
|
||||
|
||||
protected:
|
||||
JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const std::vector<Key>& keys) :
|
||||
JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const KeyVector& keys) :
|
||||
blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {}
|
||||
|
||||
friend class Marginals;
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
|
|||
|
||||
/* ************************************************************************* */
|
||||
NonlinearFactor::shared_ptr NonlinearFactor::rekey(
|
||||
const std::vector<Key>& new_keys) const {
|
||||
const KeyVector& new_keys) const {
|
||||
assert(new_keys.size() == keys().size());
|
||||
shared_ptr new_factor = clone();
|
||||
new_factor->keys() = new_keys;
|
||||
|
|
|
|||
|
|
@ -141,7 +141,7 @@ public:
|
|||
* Clones a factor and fully replaces its keys
|
||||
* @param new_keys is the full replacement set of keys
|
||||
*/
|
||||
shared_ptr rekey(const std::vector<Key>& new_keys) const;
|
||||
shared_ptr rekey(const KeyVector& new_keys) const;
|
||||
|
||||
}; // \class NonlinearFactor
|
||||
|
||||
|
|
|
|||
|
|
@ -165,10 +165,10 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
|
|||
|
||||
if (formatting.mergeSimilarFactors) {
|
||||
// Remove duplicate factors
|
||||
std::set<vector<Key> > structure;
|
||||
std::set<KeyVector > structure;
|
||||
for (const sharedFactor& factor : factors_) {
|
||||
if (factor) {
|
||||
vector<Key> factorKeys = factor->keys();
|
||||
KeyVector factorKeys = factor->keys();
|
||||
std::sort(factorKeys.begin(), factorKeys.end());
|
||||
structure.insert(factorKeys);
|
||||
}
|
||||
|
|
@ -176,7 +176,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
|
|||
|
||||
// Create factors and variable connections
|
||||
size_t i = 0;
|
||||
for(const vector<Key>& factorKeys: structure){
|
||||
for(const KeyVector& factorKeys: structure){
|
||||
// Make each factor a dot
|
||||
stm << " factor" << i << "[label=\"\", shape=point";
|
||||
{
|
||||
|
|
@ -199,7 +199,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
|
|||
for(size_t i = 0; i < size(); ++i) {
|
||||
const NonlinearFactor::shared_ptr& factor = at(i);
|
||||
if(formatting.plotFactorPoints) {
|
||||
const FastVector<Key>& keys = factor->keys();
|
||||
const KeyVector& keys = factor->keys();
|
||||
if (formatting.binaryEdges && keys.size()==2) {
|
||||
stm << " var" << keys[0] << "--" << "var" << keys[1] << ";\n";
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -47,8 +47,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
|
|||
EXPECT(!actFactor.isHessian());
|
||||
|
||||
// check keys
|
||||
FastVector<Key> expKeys; expKeys += l1, l2;
|
||||
EXPECT(assert_container_equality(expKeys, actFactor.keys()));
|
||||
EXPECT(assert_container_equality({l1, l2}, actFactor.keys()));
|
||||
|
||||
Values values;
|
||||
values.insert(l1, landmark1);
|
||||
|
|
@ -246,9 +245,7 @@ TEST( testLinearContainerFactor, creation ) {
|
|||
LinearContainerFactor actual(linear_factor, full_values);
|
||||
|
||||
// Verify the keys
|
||||
FastVector<Key> expKeys;
|
||||
expKeys += l3, l5;
|
||||
EXPECT(assert_container_equality(expKeys, actual.keys()));
|
||||
EXPECT(assert_container_equality({l3, l5}, actual.keys()));
|
||||
|
||||
// Verify subset of linearization points
|
||||
EXPECT(assert_equal(exp_values, actual.linearizationPoint(), tol));
|
||||
|
|
|
|||
|
|
@ -279,9 +279,8 @@ TEST(Values, extract_keys)
|
|||
config.insert(key3, Pose2());
|
||||
config.insert(key4, Pose2());
|
||||
|
||||
KeyVector expected, actual;
|
||||
expected += key1, key2, key3, key4;
|
||||
actual = config.keys();
|
||||
KeyVector expected {key1, key2, key3, key4};
|
||||
KeyVector actual = config.keys();
|
||||
|
||||
CHECK(actual.size() == expected.size());
|
||||
KeyVector::const_iterator itAct = actual.begin(), itExp = expected.begin();
|
||||
|
|
|
|||
|
|
@ -54,16 +54,16 @@ FastList<Key> createKeyList(std::string s, const Vector& I) {
|
|||
}
|
||||
|
||||
// Create a KeyVector from indices
|
||||
FastVector<Key> createKeyVector(const Vector& I) {
|
||||
FastVector<Key> set;
|
||||
KeyVector createKeyVector(const Vector& I) {
|
||||
KeyVector set;
|
||||
for (int i = 0; i < I.size(); i++)
|
||||
set.push_back(I[i]);
|
||||
return set;
|
||||
}
|
||||
|
||||
// Create a KeyVector from indices using symbol
|
||||
FastVector<Key> createKeyVector(std::string s, const Vector& I) {
|
||||
FastVector<Key> set;
|
||||
KeyVector createKeyVector(std::string s, const Vector& I) {
|
||||
KeyVector set;
|
||||
char c = s[0];
|
||||
for (int i = 0; i < I.size(); i++)
|
||||
set.push_back(Symbol(c, I[i]));
|
||||
|
|
@ -222,12 +222,12 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
|
|||
|
||||
/// Convert from local to world coordinates
|
||||
Values localToWorld(const Values& local, const Pose2& base,
|
||||
const FastVector<Key> user_keys = FastVector<Key>()) {
|
||||
const KeyVector user_keys = KeyVector()) {
|
||||
|
||||
Values world;
|
||||
|
||||
// if no keys given, get all keys from local values
|
||||
FastVector<Key> keys(user_keys);
|
||||
KeyVector keys(user_keys);
|
||||
if (keys.size()==0)
|
||||
keys = local.keys();
|
||||
|
||||
|
|
|
|||
|
|
@ -102,9 +102,7 @@ TEST( RangeFactor, ConstructorWithTransform) {
|
|||
|
||||
RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model,
|
||||
body_P_sensor_2D);
|
||||
KeyVector expected;
|
||||
expected.push_back(2);
|
||||
expected.push_back(1);
|
||||
KeyVector expected {2, 1};
|
||||
CHECK(factor2D.keys() == expected);
|
||||
RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model,
|
||||
body_P_sensor_3D);
|
||||
|
|
|
|||
|
|
@ -53,7 +53,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
|
|||
else
|
||||
std::cout << "Error in buildLinearOrientationGraph" << std::endl;
|
||||
|
||||
const FastVector<Key>& keys = factor->keys();
|
||||
const KeyVector& keys = factor->keys();
|
||||
Key key1 = keys[0], key2 = keys[1];
|
||||
Matrix M9 = Z_9x9;
|
||||
M9.block(0,0,3,3) = Rij;
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ public:
|
|||
}
|
||||
|
||||
/// Empty constructor with keys
|
||||
JacobianFactorQ(const FastVector<Key>& keys, //
|
||||
JacobianFactorQ(const KeyVector& keys, //
|
||||
const SharedDiagonal& model = SharedDiagonal()) :
|
||||
Base() {
|
||||
Matrix zeroMatrix = Matrix::Zero(0, D);
|
||||
|
|
@ -50,7 +50,7 @@ public:
|
|||
}
|
||||
|
||||
/// Constructor
|
||||
JacobianFactorQ(const FastVector<Key>& keys,
|
||||
JacobianFactorQ(const KeyVector& keys,
|
||||
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
|
||||
const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
|
||||
Base() {
|
||||
|
|
|
|||
|
|
@ -28,7 +28,7 @@ public:
|
|||
/**
|
||||
* Constructor
|
||||
*/
|
||||
JacobianFactorQR(const FastVector<Key>& keys,
|
||||
JacobianFactorQR(const KeyVector& keys,
|
||||
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
|
||||
const Vector& b, //
|
||||
const SharedDiagonal& model = SharedDiagonal()) :
|
||||
|
|
@ -46,7 +46,7 @@ public:
|
|||
// eliminate the point
|
||||
boost::shared_ptr<GaussianBayesNet> bn;
|
||||
GaussianFactorGraph::shared_ptr fg;
|
||||
std::vector<Key> variables;
|
||||
KeyVector variables;
|
||||
variables.push_back(pointKey);
|
||||
boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
|
||||
//fg->print("fg");
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ public:
|
|||
}
|
||||
|
||||
/// Empty constructor with keys
|
||||
JacobianFactorSVD(const FastVector<Key>& keys, //
|
||||
JacobianFactorSVD(const KeyVector& keys, //
|
||||
const SharedDiagonal& model = SharedDiagonal()) :
|
||||
Base() {
|
||||
Matrix zeroMatrix = Matrix::Zero(0, D);
|
||||
|
|
@ -58,7 +58,7 @@ public:
|
|||
*
|
||||
* @Fblocks:
|
||||
*/
|
||||
JacobianFactorSVD(const FastVector<Key>& keys,
|
||||
JacobianFactorSVD(const KeyVector& keys,
|
||||
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
|
||||
const Vector& b, //
|
||||
const SharedDiagonal& model = SharedDiagonal()) :
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@ public:
|
|||
}
|
||||
|
||||
/// Construct from blocks of F, E, inv(E'*E), and RHS vector b
|
||||
RegularImplicitSchurFactor(const FastVector<Key>& keys,
|
||||
RegularImplicitSchurFactor(const KeyVector& keys,
|
||||
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix& P,
|
||||
const Vector& b) :
|
||||
GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
|
||||
|
|
@ -108,7 +108,7 @@ public:
|
|||
return D;
|
||||
}
|
||||
|
||||
virtual void updateHessian(const FastVector<Key>& keys,
|
||||
virtual void updateHessian(const KeyVector& keys,
|
||||
SymmetricBlockMatrix* info) const {
|
||||
throw std::runtime_error(
|
||||
"RegularImplicitSchurFactor::updateHessian non implemented");
|
||||
|
|
|
|||
|
|
@ -131,7 +131,7 @@ public:
|
|||
/**
|
||||
* Add a bunch of measurements, together with the camera keys
|
||||
*/
|
||||
void add(ZVector& measurements, std::vector<Key>& cameraKeys) {
|
||||
void add(ZVector& measurements, KeyVector& cameraKeys) {
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
this->measured_.push_back(measurements.at(i));
|
||||
this->keys_.push_back(cameraKeys.at(i));
|
||||
|
|
@ -310,7 +310,7 @@ public:
|
|||
void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
|
||||
const double lambda, bool diagonalDamping,
|
||||
SymmetricBlockMatrix& augmentedHessian,
|
||||
const FastVector<Key> allKeys) const {
|
||||
const KeyVector allKeys) const {
|
||||
Matrix E;
|
||||
Vector b;
|
||||
computeJacobians(Fs, E, b, cameras, point);
|
||||
|
|
|
|||
|
|
@ -179,7 +179,7 @@ public:
|
|||
|
||||
size_t numKeys = this->keys_.size();
|
||||
// Create structures for Hessian Factors
|
||||
std::vector<Key> js;
|
||||
KeyVector js;
|
||||
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
|
||||
std::vector<Vector> gs(numKeys);
|
||||
|
||||
|
|
|
|||
|
|
@ -168,14 +168,14 @@ GaussianFactorGraph buildLinearOrientationGraph(
|
|||
|
||||
// put original measurements in the spanning tree
|
||||
for(const size_t& factorId: spanningTreeIds) {
|
||||
const FastVector<Key>& keys = g[factorId]->keys();
|
||||
const KeyVector& keys = g[factorId]->keys();
|
||||
Key key1 = keys[0], key2 = keys[1];
|
||||
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
|
||||
lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
|
||||
}
|
||||
// put regularized measurements in the chordsIds
|
||||
for(const size_t& factorId: chordsIds) {
|
||||
const FastVector<Key>& keys = g[factorId]->keys();
|
||||
const KeyVector& keys = g[factorId]->keys();
|
||||
Key key1 = keys[0], key2 = keys[1];
|
||||
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
|
||||
double key1_DeltaTheta_key2 = deltaTheta(0);
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@ const Matrix26 F0 = Matrix26::Ones();
|
|||
const Matrix26 F1 = 2 * Matrix26::Ones();
|
||||
const Matrix26 F3 = 3 * Matrix26::Ones();
|
||||
const vector<Matrix26, Eigen::aligned_allocator<Matrix26> > FBlocks = list_of<Matrix26>(F0)(F1)(F3);
|
||||
const FastVector<Key> keys = list_of<Key>(0)(1)(3);
|
||||
const KeyVector keys {0, 1, 3};
|
||||
// RHS and sigmas
|
||||
const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
|
||||
|
||||
|
|
@ -86,8 +86,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
|
|||
F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3;
|
||||
|
||||
// Calculate expected result F'*alpha*(I - E*P*E')*F*x
|
||||
FastVector<Key> keys2;
|
||||
keys2 += 0,1,2,3;
|
||||
KeyVector keys2{0,1,2,3};
|
||||
Vector x = xvalues.vector(keys2);
|
||||
Vector expected = Vector::Zero(24);
|
||||
RegularImplicitSchurFactor<CalibratedCamera>::multiplyHessianAdd(F, E, P, alpha, x, expected);
|
||||
|
|
|
|||
|
|
@ -170,9 +170,7 @@ TEST( SmartProjectionCameraFactor, noisy ) {
|
|||
measurements.push_back(level_uv);
|
||||
measurements.push_back(level_uv_right);
|
||||
|
||||
vector<Key> views;
|
||||
views.push_back(c1);
|
||||
views.push_back(c2);
|
||||
KeyVector views {c1, c2};
|
||||
|
||||
factor2->add(measurements, views);
|
||||
|
||||
|
|
@ -195,10 +193,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
|
|||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
|
||||
vector<Key> views;
|
||||
views.push_back(c1);
|
||||
views.push_back(c2);
|
||||
views.push_back(c3);
|
||||
KeyVector views {c1, c2, c3};
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
|
@ -293,10 +288,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
vector<Key> views;
|
||||
views.push_back(c1);
|
||||
views.push_back(c2);
|
||||
views.push_back(c3);
|
||||
KeyVector views {c1, c2, c3};
|
||||
|
||||
SfM_Track track1;
|
||||
for (size_t i = 0; i < 3; ++i) {
|
||||
|
|
@ -370,10 +362,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
|
||||
|
||||
vector<Key> views;
|
||||
views.push_back(c1);
|
||||
views.push_back(c2);
|
||||
views.push_back(c3);
|
||||
KeyVector views {c1, c2, c3};
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
|
@ -450,10 +439,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
|
||||
|
||||
vector<Key> views;
|
||||
views.push_back(c1);
|
||||
views.push_back(c2);
|
||||
views.push_back(c3);
|
||||
KeyVector views {c1, c2, c3};
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
|
@ -526,10 +512,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
|
||||
|
||||
vector<Key> views;
|
||||
views.push_back(c1);
|
||||
views.push_back(c2);
|
||||
views.push_back(c3);
|
||||
KeyVector views {c1, c2, c3};
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
|
|
|||
|
|
@ -189,7 +189,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
|
|||
measurements.push_back(level_uv);
|
||||
measurements.push_back(level_uv_right);
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
|
||||
|
|
@ -236,7 +236,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
// Create smart factors
|
||||
std::vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -299,7 +299,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -370,7 +370,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
|
|||
measurements_cam1.push_back(cam2.project(landmark1));
|
||||
|
||||
// Create smart factors
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
|
||||
|
|
@ -459,7 +459,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
|
|||
b.setZero();
|
||||
|
||||
// Create smart factors
|
||||
FastVector<Key> keys;
|
||||
KeyVector keys;
|
||||
keys.push_back(x1);
|
||||
keys.push_back(x2);
|
||||
|
||||
|
|
@ -520,7 +520,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
|
|||
|
||||
using namespace vanillaPose;
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -577,7 +577,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
|
|||
|
||||
using namespace vanillaPose;
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -638,7 +638,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
|
|||
|
||||
double excludeLandmarksFutherThanDist = 2;
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -701,7 +701,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
double excludeLandmarksFutherThanDist = 1e10;
|
||||
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -767,7 +767,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
|
|||
|
||||
using namespace vanillaPose;
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -821,7 +821,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
|
|||
|
||||
using namespace vanillaPose2;
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -869,7 +869,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
|
|||
/* *************************************************************************/
|
||||
TEST( SmartProjectionPoseFactor, CheckHessian) {
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -955,7 +955,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
|
|||
TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) {
|
||||
using namespace vanillaPose2;
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -1014,7 +1014,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
|
|||
// this test considers a condition in which the cheirality constraint is triggered
|
||||
using namespace vanillaPose;
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -1099,7 +1099,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
|
|||
|
||||
using namespace vanillaPose2;
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
|
||||
|
|
@ -1133,7 +1133,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
|
|||
|
||||
using namespace vanillaPose;
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -1186,7 +1186,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
|
|||
|
||||
using namespace vanillaPose2;
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -1260,7 +1260,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -1309,7 +1309,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
|
|||
|
||||
using namespace bundlerPose;
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -1421,7 +1421,7 @@ TEST(SmartProjectionPoseFactor, serialize2) {
|
|||
SmartFactor factor(model, sharedK, bts, params);
|
||||
|
||||
// insert some measurments
|
||||
vector<Key> key_view;
|
||||
KeyVector key_view;
|
||||
Point2Vector meas_view;
|
||||
key_view.push_back(Symbol('x', 1));
|
||||
meas_view.push_back(Point2(10, 10));
|
||||
|
|
|
|||
|
|
@ -57,7 +57,7 @@ namespace gtsam
|
|||
const size_t nFrontals = keys.size();
|
||||
|
||||
// Build a key vector with the frontals followed by the separator
|
||||
FastVector<Key> orderedKeys(allKeys.size());
|
||||
KeyVector orderedKeys(allKeys.size());
|
||||
std::copy(keys.begin(), keys.end(), orderedKeys.begin());
|
||||
std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);
|
||||
|
||||
|
|
|
|||
|
|
@ -81,10 +81,7 @@ TEST( SymbolicBayesNet, combine )
|
|||
TEST(SymbolicBayesNet, saveGraph) {
|
||||
SymbolicBayesNet bn;
|
||||
bn += SymbolicConditional(_A_, _B_);
|
||||
std::vector<Key> keys;
|
||||
keys.push_back(_B_);
|
||||
keys.push_back(_C_);
|
||||
keys.push_back(_D_);
|
||||
KeyVector keys {_B_, _C_, _D_};
|
||||
bn += SymbolicConditional::FromKeys(keys,2);
|
||||
bn += SymbolicConditional(_D_);
|
||||
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@ using namespace boost::assign;
|
|||
/* ************************************************************************* */
|
||||
#ifdef TRACK_ELIMINATE
|
||||
TEST(SymbolicFactor, eliminate) {
|
||||
vector<Key> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
|
||||
KeyVector keys {2, 3, 4, 6, 7, 9, 10, 11};
|
||||
IndexFactor actual(keys.begin(), keys.end());
|
||||
BayesNet<IndexConditional> fragment = *actual.eliminate(3);
|
||||
|
||||
|
|
|
|||
|
|
@ -85,7 +85,7 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential)
|
|||
SymbolicBayesNet::shared_ptr actualBayesNet2;
|
||||
SymbolicFactorGraph::shared_ptr actualSfg2;
|
||||
boost::tie(actualBayesNet2, actualSfg2) =
|
||||
simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container<vector<Key> >());
|
||||
simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container<KeyVector >());
|
||||
|
||||
EXPECT(assert_equal(expectedSfg, *actualSfg2));
|
||||
EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2));
|
||||
|
|
@ -137,7 +137,7 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal)
|
|||
SymbolicBayesTree::shared_ptr actualBayesTree2;
|
||||
SymbolicFactorGraph::shared_ptr actualFactorGraph2;
|
||||
boost::tie(actualBayesTree2, actualFactorGraph2) =
|
||||
simpleTestGraph2.eliminatePartialMultifrontal(list_of<Key>(4)(5).convert_to_container<vector<Key> >());
|
||||
simpleTestGraph2.eliminatePartialMultifrontal(list_of<Key>(4)(5).convert_to_container<KeyVector >());
|
||||
|
||||
EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2));
|
||||
EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2));
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@ namespace gtsam {
|
|||
public:
|
||||
|
||||
/** A map from keys to values */
|
||||
typedef std::vector<Key> Indices;
|
||||
typedef KeyVector Indices;
|
||||
typedef Assignment<Key> Values;
|
||||
typedef boost::shared_ptr<Values> sharedValues;
|
||||
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ namespace gtsam {
|
|||
protected:
|
||||
|
||||
/// Construct n-way factor
|
||||
Constraint(const std::vector<Key>& js) :
|
||||
Constraint(const KeyVector& js) :
|
||||
DiscreteFactor(js) {
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -57,7 +57,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Domain::checkAllDiff(const vector<Key> keys, vector<Domain>& domains) {
|
||||
bool Domain::checkAllDiff(const KeyVector keys, vector<Domain>& domains) {
|
||||
Key j = keys_[0];
|
||||
// for all values in this domain
|
||||
for(size_t value: values_) {
|
||||
|
|
|
|||
|
|
@ -104,7 +104,7 @@ namespace gtsam {
|
|||
* If found, we make this a singleton... Called in AllDiff::ensureArcConsistency
|
||||
* @param keys connected domains through alldiff
|
||||
*/
|
||||
bool checkAllDiff(const std::vector<Key> keys, std::vector<Domain>& domains);
|
||||
bool checkAllDiff(const KeyVector keys, std::vector<Domain>& domains);
|
||||
|
||||
/// Partially apply known values
|
||||
virtual Constraint::shared_ptr partiallyApply(
|
||||
|
|
|
|||
|
|
@ -203,9 +203,9 @@ void RawQP::addQuadTerm(
|
|||
}
|
||||
|
||||
QP RawQP::makeQP() {
|
||||
std::vector < Key > keys;
|
||||
std::vector < Matrix > Gs;
|
||||
std::vector < Vector > gs;
|
||||
KeyVector keys;
|
||||
std::vector<Matrix> Gs;
|
||||
std::vector<Vector> gs;
|
||||
for (auto kv : varname_to_key) {
|
||||
keys.push_back(kv.second);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@ private:
|
|||
std::string name_;
|
||||
std::unordered_map<Key, double> up;
|
||||
std::unordered_map<Key, double> lo;
|
||||
std::vector<Key> Free;
|
||||
KeyVector Free;
|
||||
const bool debug = false;
|
||||
|
||||
public:
|
||||
|
|
|
|||
|
|
@ -362,7 +362,7 @@ void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysT
|
|||
|
||||
// COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
|
||||
if(keysToMove && keysToMove->size() > 0) {
|
||||
ordering_ = Ordering::ColamdConstrainedFirst(factors_, std::vector<Key>(keysToMove->begin(), keysToMove->end()));
|
||||
ordering_ = Ordering::ColamdConstrainedFirst(factors_, KeyVector(keysToMove->begin(), keysToMove->end()));
|
||||
}else{
|
||||
ordering_ = Ordering::Colamd(factors_);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() {
|
|||
variableIndex_ = VariableIndex(factors_);
|
||||
|
||||
KeyVector separatorKeys = separatorValues_.keys();
|
||||
ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, std::vector<Key>(separatorKeys.begin(), separatorKeys.end()));
|
||||
ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, KeyVector(separatorKeys.begin(), separatorKeys.end()));
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -72,7 +72,7 @@ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph,
|
|||
GaussianFactorGraph linearFactorGraph = *graph.linearize(theta);
|
||||
// .first is the eliminated Bayes tree, while .second is the remaining factor graph
|
||||
GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal(
|
||||
std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end()), eliminateFunction).second;
|
||||
KeyVector(marginalizeKeys.begin(), marginalizeKeys.end()), eliminateFunction).second;
|
||||
|
||||
// Wrap in nonlinear container factors
|
||||
NonlinearFactorGraph marginalFactors;
|
||||
|
|
|
|||
|
|
@ -353,7 +353,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
|
|||
}
|
||||
|
||||
// Create the set of clique keys LC:
|
||||
std::vector<Key> cliqueKeys;
|
||||
KeyVector cliqueKeys;
|
||||
for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
|
||||
for(Key key: clique->conditional()->frontals()) {
|
||||
cliqueKeys.push_back(key);
|
||||
|
|
|
|||
|
|
@ -194,7 +194,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
|
|||
}
|
||||
|
||||
// Create the set of clique keys LC:
|
||||
std::vector<Key> cliqueKeys;
|
||||
KeyVector cliqueKeys;
|
||||
for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
|
||||
for(Key key: clique->conditional()->frontals()) {
|
||||
cliqueKeys.push_back(key);
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@ class NonlinearClusterTree : public ClusterTree<NonlinearFactorGraph> {
|
|||
// Given graph, index, add factors with specified keys into
|
||||
// Factors are erased in the graph
|
||||
// TODO(frank): fairly hacky and inefficient. Think about iterating the graph once instead
|
||||
NonlinearCluster(const VariableIndex& variableIndex, const std::vector<Key>& keys,
|
||||
NonlinearCluster(const VariableIndex& variableIndex, const KeyVector& keys,
|
||||
NonlinearFactorGraph* graph) {
|
||||
for (const Key key : keys) {
|
||||
std::vector<NonlinearFactor::shared_ptr> factors;
|
||||
|
|
|
|||
|
|
@ -81,7 +81,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
|
|||
|
||||
GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
|
||||
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
|
||||
|
||||
NonlinearFactorGraph LinearContainerForGaussianMarginals;
|
||||
for(const GaussianFactor::shared_ptr& factor: marginal) {
|
||||
|
|
@ -419,9 +419,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
|
|||
ordering.push_back(3);
|
||||
|
||||
// Create the set of marginalizable variables
|
||||
std::vector<Key> linearIndices;
|
||||
linearIndices.push_back(1);
|
||||
linearIndices.push_back(2);
|
||||
KeyVector linearIndices {1, 2};
|
||||
|
||||
GaussianFactorGraph linearPartialGraph = *partialGraph.linearize(partialValues);
|
||||
GaussianFactorGraph result = *linearPartialGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
|
||||
|
|
@ -1008,8 +1006,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_1 )
|
|||
GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues);
|
||||
|
||||
// Create the set of marginalizable variables
|
||||
std::vector<Key> linearIndices;
|
||||
linearIndices.push_back(1);
|
||||
KeyVector linearIndices {1};
|
||||
|
||||
GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
|
||||
|
||||
|
|
@ -1062,9 +1059,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 )
|
|||
GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues);
|
||||
|
||||
// Create the set of marginalizable variables
|
||||
std::vector<Key> linearIndices;
|
||||
linearIndices.push_back(1);
|
||||
linearIndices.push_back(2);
|
||||
KeyVector linearIndices {1, 2};
|
||||
|
||||
GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
|
||||
|
||||
|
|
|
|||
|
|
@ -563,7 +563,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
|
|||
for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
|
||||
eliminateKeys.erase(key_value.key);
|
||||
}
|
||||
std::vector<Key> variables(eliminateKeys.begin(), eliminateKeys.end());
|
||||
KeyVector variables(eliminateKeys.begin(), eliminateKeys.end());
|
||||
GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second;
|
||||
|
||||
expectedSmootherSummarization.resize(0);
|
||||
|
|
|
|||
|
|
@ -98,7 +98,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
|
|||
|
||||
GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
|
||||
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
|
||||
|
||||
NonlinearFactorGraph LinearContainerForGaussianMarginals;
|
||||
for(const GaussianFactor::shared_ptr& factor: marginal) {
|
||||
|
|
@ -429,7 +429,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
|
|||
|
||||
GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues);
|
||||
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
|
||||
|
||||
NonlinearFactorGraph expectedGraph;
|
||||
|
||||
|
|
@ -513,7 +513,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
|
|||
|
||||
GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues);
|
||||
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
|
||||
|
||||
NonlinearFactorGraph expectedGraph;
|
||||
|
||||
|
|
@ -1108,17 +1108,15 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
|
|||
newValues.insert(3, value3);
|
||||
|
||||
// Create the set of marginalizable variables
|
||||
std::vector<Key> linearIndices;
|
||||
linearIndices.push_back(1);
|
||||
|
||||
GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
|
||||
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
|
||||
KeyVector linearIndices {1};
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
|
||||
|
||||
NonlinearFactorGraph expectedMarginals;
|
||||
for(const GaussianFactor::shared_ptr& factor: marginal) {
|
||||
expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
|
||||
}
|
||||
NonlinearFactorGraph expectedMarginals;
|
||||
for(const GaussianFactor::shared_ptr& factor: marginal) {
|
||||
expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
|
||||
}
|
||||
|
||||
FastList<Key> keysToMarginalize;
|
||||
keysToMarginalize.push_back(1);
|
||||
|
|
@ -1156,15 +1154,10 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
|
|||
newValues.insert(3, value3);
|
||||
|
||||
|
||||
// Create the set of marginalizable variables
|
||||
std::vector<Key> linearIndices;
|
||||
linearIndices.push_back(1);
|
||||
linearIndices.push_back(2);
|
||||
|
||||
|
||||
// Create the set of marginalizable variables
|
||||
KeyVector linearIndices {1, 2};
|
||||
GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
|
||||
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
|
||||
|
||||
NonlinearFactorGraph expectedMarginals;
|
||||
for(const GaussianFactor::shared_ptr& factor: marginal) {
|
||||
|
|
|
|||
|
|
@ -583,7 +583,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
|
|||
for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
|
||||
allkeys.erase(key_value.key);
|
||||
}
|
||||
std::vector<Key> variables(allkeys.begin(), allkeys.end());
|
||||
KeyVector variables(allkeys.begin(), allkeys.end());
|
||||
std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
|
||||
|
||||
expectedSmootherSummarization.resize(0);
|
||||
|
|
|
|||
|
|
@ -584,7 +584,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
|
|||
KeySet allkeys = LinFactorGraph->keys();
|
||||
for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues)
|
||||
allkeys.erase(key_value.key);
|
||||
std::vector<Key> variables(allkeys.begin(), allkeys.end());
|
||||
KeyVector variables(allkeys.begin(), allkeys.end());
|
||||
std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
|
||||
|
||||
expectedSmootherSummarization.resize(0);
|
||||
|
|
|
|||
|
|
@ -40,10 +40,10 @@ public:
|
|||
private:
|
||||
|
||||
typedef BetweenFactorEM<VALUE> This;
|
||||
typedef gtsam::NonlinearFactor Base;
|
||||
typedef NonlinearFactor Base;
|
||||
|
||||
gtsam::Key key1_;
|
||||
gtsam::Key key2_;
|
||||
Key key1_;
|
||||
Key key2_;
|
||||
|
||||
VALUE measured_; /** The measurement */
|
||||
|
||||
|
|
@ -114,38 +114,38 @@ public:
|
|||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/* ************************************************************************* */
|
||||
virtual double error(const gtsam::Values& x) const {
|
||||
virtual double error(const Values& x) const {
|
||||
return whitenedError(x).squaredNorm();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
|
||||
* Linearize a non-linearFactorN to get a GaussianFactor,
|
||||
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
|
||||
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
|
||||
*/
|
||||
/* This version of linearize recalculates the noise model each time */
|
||||
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(
|
||||
const gtsam::Values& x) const {
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(
|
||||
const Values& x) const {
|
||||
// Only linearize if the factor is active
|
||||
if (!this->active(x))
|
||||
return boost::shared_ptr<gtsam::JacobianFactor>();
|
||||
return boost::shared_ptr<JacobianFactor>();
|
||||
|
||||
//std::cout<<"About to linearize"<<std::endl;
|
||||
gtsam::Matrix A1, A2;
|
||||
std::vector<gtsam::Matrix> A(this->size());
|
||||
gtsam::Vector b = -whitenedError(x, A);
|
||||
Matrix A1, A2;
|
||||
std::vector<Matrix> A(this->size());
|
||||
Vector b = -whitenedError(x, A);
|
||||
A1 = A[0];
|
||||
A2 = A[1];
|
||||
|
||||
return gtsam::GaussianFactor::shared_ptr(
|
||||
new gtsam::JacobianFactor(key1_, A1, key2_, A2, b,
|
||||
gtsam::noiseModel::Unit::Create(b.size())));
|
||||
return GaussianFactor::shared_ptr(
|
||||
new JacobianFactor(key1_, A1, key2_, A2, b,
|
||||
noiseModel::Unit::Create(b.size())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Vector whitenedError(const gtsam::Values& x,
|
||||
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
|
||||
Vector whitenedError(const Values& x,
|
||||
boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
||||
|
||||
bool debug = true;
|
||||
|
||||
|
|
@ -181,11 +181,11 @@ public:
|
|||
|
||||
Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1);
|
||||
Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1);
|
||||
Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier);
|
||||
Matrix H1_aug = stack(2, &H1_inlier, &H1_outlier);
|
||||
|
||||
Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2);
|
||||
Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2);
|
||||
Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier);
|
||||
Matrix H2_aug = stack(2, &H2_inlier, &H2_outlier);
|
||||
|
||||
(*H)[0].resize(H1_aug.rows(), H1_aug.cols());
|
||||
(*H)[1].resize(H2_aug.rows(), H2_aug.cols());
|
||||
|
|
@ -229,7 +229,7 @@ public:
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const {
|
||||
Vector calcIndicatorProb(const Values& x) const {
|
||||
|
||||
bool debug = false;
|
||||
|
||||
|
|
@ -285,7 +285,7 @@ public:
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
|
||||
Vector unwhitenedError(const Values& x) const {
|
||||
|
||||
const T& p1 = x.at<T>(key1_);
|
||||
const T& p2 = x.at<T>(key2_);
|
||||
|
|
@ -328,8 +328,8 @@ public:
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void updateNoiseModels(const gtsam::Values& values,
|
||||
const gtsam::NonlinearFactorGraph& graph) {
|
||||
void updateNoiseModels(const Values& values,
|
||||
const NonlinearFactorGraph& graph) {
|
||||
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
|
||||
* (note these are given in the E step, where indicator probabilities are calculated).
|
||||
*
|
||||
|
|
@ -340,7 +340,7 @@ public:
|
|||
*/
|
||||
|
||||
// get joint covariance of the involved states
|
||||
std::vector<gtsam::Key> Keys;
|
||||
KeyVector Keys;
|
||||
Keys.push_back(key1_);
|
||||
Keys.push_back(key2_);
|
||||
Marginals marginals(graph, values, Marginals::QR);
|
||||
|
|
@ -353,7 +353,7 @@ public:
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void updateNoiseModels_givenCovs(const gtsam::Values& values,
|
||||
void updateNoiseModels_givenCovs(const Values& values,
|
||||
const Matrix& cov1, const Matrix& cov2, const Matrix& cov12) {
|
||||
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
|
||||
* (note these are given in the E step, where indicator probabilities are calculated).
|
||||
|
|
@ -385,12 +385,12 @@ public:
|
|||
// update inlier and outlier noise models
|
||||
Matrix covRinlier =
|
||||
(model_inlier_->R().transpose() * model_inlier_->R()).inverse();
|
||||
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(
|
||||
model_inlier_ = noiseModel::Gaussian::Covariance(
|
||||
covRinlier + cov_state);
|
||||
|
||||
Matrix covRoutlier =
|
||||
(model_outlier_->R().transpose() * model_outlier_->R()).inverse();
|
||||
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(
|
||||
model_outlier_ = noiseModel::Gaussian::Covariance(
|
||||
covRoutlier + cov_state);
|
||||
|
||||
// model_inlier_->print("after:");
|
||||
|
|
@ -426,4 +426,4 @@ private:
|
|||
};
|
||||
// \class BetweenFactorEM
|
||||
|
||||
}/// namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -202,7 +202,7 @@ public:
|
|||
|
||||
size_t numKeys = this->keys_.size();
|
||||
// Create structures for Hessian Factors
|
||||
std::vector<Key> js;
|
||||
KeyVector js;
|
||||
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
|
||||
std::vector<Vector> gs(numKeys);
|
||||
|
||||
|
|
|
|||
|
|
@ -92,7 +92,7 @@ public:
|
|||
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
|
||||
* @param Ks vector of calibration objects
|
||||
*/
|
||||
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
|
||||
void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
|
||||
std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
|
||||
Base::add(measurements, poseKeys);
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
|
|
@ -106,7 +106,7 @@ public:
|
|||
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
|
||||
* @param K the (known) camera calibration (same for all measurements)
|
||||
*/
|
||||
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
|
||||
void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
|
||||
const boost::shared_ptr<Cal3_S2Stereo> K) {
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
Base::add(measurements.at(i), poseKeys.at(i));
|
||||
|
|
|
|||
|
|
@ -43,16 +43,16 @@ namespace gtsam {
|
|||
private:
|
||||
|
||||
typedef TransformBtwRobotsUnaryFactorEM<VALUE> This;
|
||||
typedef gtsam::NonlinearFactor Base;
|
||||
typedef NonlinearFactor Base;
|
||||
|
||||
gtsam::Key key_;
|
||||
Key key_;
|
||||
|
||||
VALUE measured_; /** The measurement */
|
||||
|
||||
gtsam::Values valA_; // given values for robot A map\trajectory
|
||||
gtsam::Values valB_; // given values for robot B map\trajectory
|
||||
gtsam::Key keyA_; // key of robot A to which the measurement refers
|
||||
gtsam::Key keyB_; // key of robot B to which the measurement refers
|
||||
Values valA_; // given values for robot A map\trajectory
|
||||
Values valB_; // given values for robot B map\trajectory
|
||||
Key keyA_; // key of robot A to which the measurement refers
|
||||
Key keyB_; // key of robot B to which the measurement refers
|
||||
|
||||
// TODO: create function to update valA_ and valB_
|
||||
|
||||
|
|
@ -79,7 +79,7 @@ namespace gtsam {
|
|||
|
||||
/** Constructor */
|
||||
TransformBtwRobotsUnaryFactorEM(Key key, const VALUE& measured, Key keyA, Key keyB,
|
||||
const gtsam::Values& valA, const gtsam::Values& valB,
|
||||
const Values& valA, const Values& valB,
|
||||
const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
|
||||
const double prior_inlier, const double prior_outlier,
|
||||
const bool flag_bump_up_near_zero_probs = false,
|
||||
|
|
@ -97,7 +97,7 @@ namespace gtsam {
|
|||
|
||||
|
||||
/** Clone */
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
|
||||
virtual NonlinearFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
|
||||
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
|
|
@ -134,7 +134,7 @@ namespace gtsam {
|
|||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/* ************************************************************************* */
|
||||
void setValAValB(const gtsam::Values& valA, const gtsam::Values& valB){
|
||||
void setValAValB(const Values& valA, const Values& valB){
|
||||
if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) )
|
||||
throw("something is wrong!");
|
||||
|
||||
|
|
@ -151,36 +151,36 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
virtual double error(const gtsam::Values& x) const {
|
||||
virtual double error(const Values& x) const {
|
||||
return whitenedError(x).squaredNorm();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
|
||||
* Linearize a non-linearFactorN to get a GaussianFactor,
|
||||
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
|
||||
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
|
||||
*/
|
||||
/* This version of linearize recalculates the noise model each time */
|
||||
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const {
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
|
||||
// Only linearize if the factor is active
|
||||
if (!this->active(x))
|
||||
return boost::shared_ptr<gtsam::JacobianFactor>();
|
||||
return boost::shared_ptr<JacobianFactor>();
|
||||
|
||||
//std::cout<<"About to linearize"<<std::endl;
|
||||
gtsam::Matrix A1;
|
||||
std::vector<gtsam::Matrix> A(this->size());
|
||||
gtsam::Vector b = -whitenedError(x, A);
|
||||
Matrix A1;
|
||||
std::vector<Matrix> A(this->size());
|
||||
Vector b = -whitenedError(x, A);
|
||||
A1 = A[0];
|
||||
|
||||
return gtsam::GaussianFactor::shared_ptr(
|
||||
new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(b.size())));
|
||||
return GaussianFactor::shared_ptr(
|
||||
new JacobianFactor(key_, A1, b, noiseModel::Unit::Create(b.size())));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Vector whitenedError(const gtsam::Values& x,
|
||||
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
|
||||
Vector whitenedError(const Values& x,
|
||||
boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
||||
|
||||
bool debug = true;
|
||||
|
||||
|
|
@ -227,7 +227,7 @@ namespace gtsam {
|
|||
|
||||
Matrix H_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H_unwh);
|
||||
Matrix H_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H_unwh);
|
||||
Matrix H_aug = gtsam::stack(2, &H_inlier, &H_outlier);
|
||||
Matrix H_aug = stack(2, &H_inlier, &H_outlier);
|
||||
|
||||
(*H)[0].resize(H_aug.rows(),H_aug.cols());
|
||||
(*H)[0] = H_aug;
|
||||
|
|
@ -246,7 +246,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const {
|
||||
Vector calcIndicatorProb(const Values& x) const {
|
||||
|
||||
Vector err = unwhitenedError(x);
|
||||
|
||||
|
|
@ -254,7 +254,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Vector calcIndicatorProb(const gtsam::Values& x, const gtsam::Vector& err) const {
|
||||
Vector calcIndicatorProb(const Values& x, const Vector& err) const {
|
||||
|
||||
// Calculate indicator probabilities (inlier and outlier)
|
||||
Vector err_wh_inlier = model_inlier_->whiten(err);
|
||||
|
|
@ -288,7 +288,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
|
||||
Vector unwhitenedError(const Values& x) const {
|
||||
|
||||
T orgA_T_currA = valA_.at<T>(keyA_);
|
||||
T orgB_T_currB = valB_.at<T>(keyB_);
|
||||
|
|
@ -325,10 +325,10 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void updateNoiseModels(const gtsam::Values& values, const gtsam::Marginals& marginals) {
|
||||
void updateNoiseModels(const Values& values, const Marginals& marginals) {
|
||||
/* given marginals version, don't need to marginal multiple times if update a lot */
|
||||
|
||||
std::vector<gtsam::Key> Keys;
|
||||
KeyVector Keys;
|
||||
Keys.push_back(keyA_);
|
||||
Keys.push_back(keyB_);
|
||||
JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
|
||||
|
|
@ -340,7 +340,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
|
||||
void updateNoiseModels(const Values& values, const NonlinearFactorGraph& graph){
|
||||
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
|
||||
* (note these are given in the E step, where indicator probabilities are calculated).
|
||||
*
|
||||
|
|
@ -358,7 +358,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
|
||||
void updateNoiseModels_givenCovs(const Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
|
||||
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
|
||||
* (note these are given in the E step, where indicator probabilities are calculated).
|
||||
*
|
||||
|
|
@ -389,10 +389,10 @@ namespace gtsam {
|
|||
|
||||
// update inlier and outlier noise models
|
||||
Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
|
||||
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
|
||||
model_inlier_ = noiseModel::Gaussian::Covariance(covRinlier + cov_state);
|
||||
|
||||
Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
|
||||
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
|
||||
model_outlier_ = noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
|
||||
|
||||
// model_inlier_->print("after:");
|
||||
// std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
|
||||
|
|
|
|||
|
|
@ -274,7 +274,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
|
|||
Ks.push_back(K);
|
||||
Ks.push_back(K);
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
|
||||
|
|
@ -313,7 +313,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
|
|||
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark3);
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -455,7 +455,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) {
|
|||
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark3);
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -548,7 +548,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
// Create smart factors
|
||||
std::vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -614,7 +614,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
|
|||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -680,7 +680,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
|
|||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) {
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -754,7 +754,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
|
|||
|
||||
// double excludeLandmarksFutherThanDist = 2;
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -822,7 +822,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
|
|||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -919,7 +919,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
///* *************************************************************************/
|
||||
//TEST( SmartStereoProjectionPoseFactor, jacobianQ ){
|
||||
//
|
||||
// vector<Key> views;
|
||||
// KeyVector views;
|
||||
// views.push_back(x1);
|
||||
// views.push_back(x2);
|
||||
// views.push_back(x3);
|
||||
|
|
@ -980,7 +980,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
///* *************************************************************************/
|
||||
//TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){
|
||||
//
|
||||
// vector<Key> views;
|
||||
// KeyVector views;
|
||||
// views.push_back(x1);
|
||||
// views.push_back(x2);
|
||||
// views.push_back(x3);
|
||||
|
|
@ -1040,7 +1040,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -1128,7 +1128,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
|
|||
///* *************************************************************************/
|
||||
//TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
|
||||
//
|
||||
// vector<Key> views;
|
||||
// KeyVector views;
|
||||
// views.push_back(x1);
|
||||
// views.push_back(x2);
|
||||
// views.push_back(x3);
|
||||
|
|
@ -1194,7 +1194,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
|
|||
///* *************************************************************************/
|
||||
//TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
|
||||
//
|
||||
// vector<Key> views;
|
||||
// KeyVector views;
|
||||
// views.push_back(x1);
|
||||
// views.push_back(x2);
|
||||
// views.push_back(x3);
|
||||
|
|
@ -1268,7 +1268,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
|
|||
///* *************************************************************************/
|
||||
//TEST( SmartStereoProjectionPoseFactor, Hessian ){
|
||||
//
|
||||
// vector<Key> views;
|
||||
// KeyVector views;
|
||||
// views.push_back(x1);
|
||||
// views.push_back(x2);
|
||||
//
|
||||
|
|
@ -1309,7 +1309,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
|
|||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
@ -1379,7 +1379,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
|
|||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) {
|
||||
|
||||
vector<Key> views;
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
|
|
|||
|
|
@ -231,7 +231,7 @@ TEST(ExpressionFactor, Shallow) {
|
|||
Point2_ expression = project(transform_to(x_, p_));
|
||||
|
||||
// Get and check keys and dims
|
||||
FastVector<Key> keys;
|
||||
KeyVector keys;
|
||||
FastVector<int> dims;
|
||||
boost::tie(keys, dims) = expression.keysAndDims();
|
||||
LONGS_EQUAL(2,keys.size());
|
||||
|
|
|
|||
|
|
@ -648,7 +648,7 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check)
|
|||
namespace {
|
||||
bool checkMarginalizeLeaves(ISAM2& isam, const FastList<Key>& leafKeys) {
|
||||
Matrix expectedAugmentedHessian, expected3AugmentedHessian;
|
||||
vector<Key> toKeep;
|
||||
KeyVector toKeep;
|
||||
for(Key j: isam.getDelta() | br::map_keys)
|
||||
if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end())
|
||||
toKeep.push_back(j);
|
||||
|
|
|
|||
|
|
@ -139,10 +139,7 @@ TEST(Marginals, planarSLAMmarginals) {
|
|||
0.151935669757191, 0.007741936219615, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.160967924878730, 0.007741936219615, 0.004516127560770,
|
||||
-0.104516127560770, 0.351935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193,
|
||||
-0.050967744878460, 0.056129031890193, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.004516127560770, 0.056129031890193, 0.027741936219615;
|
||||
vector<Key> variables(3);
|
||||
variables[0] = x1;
|
||||
variables[1] = l2;
|
||||
variables[2] = x3;
|
||||
KeyVector variables {x1, l2, x3};
|
||||
JointMarginal joint_l2x1x3 = marginals.jointMarginalCovariance(variables);
|
||||
EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,0,2,2)), Matrix(joint_l2x1x3(l2,l2)), 1e-6));
|
||||
EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,0,3,2)), Matrix(joint_l2x1x3(x1,l2)), 1e-6));
|
||||
|
|
@ -227,7 +224,7 @@ TEST(Marginals, order) {
|
|||
|
||||
Marginals marginals(fg, vals);
|
||||
KeySet set = fg.keys();
|
||||
FastVector<Key> keys(set.begin(), set.end());
|
||||
KeyVector keys(set.begin(), set.end());
|
||||
JointMarginal joint = marginals.jointMarginalCovariance(keys);
|
||||
|
||||
LONGS_EQUAL(3, (long)joint(0,0).rows());
|
||||
|
|
|
|||
|
|
@ -401,11 +401,7 @@ TEST( NonlinearFactor, clone_rekey )
|
|||
EXPECT(assert_equal(*init, *actClone));
|
||||
|
||||
// Re-key factor - clones with different keys
|
||||
std::vector<Key> new_keys(4);
|
||||
new_keys[0] = X(5);
|
||||
new_keys[1] = X(6);
|
||||
new_keys[2] = X(7);
|
||||
new_keys[3] = X(8);
|
||||
KeyVector new_keys {X(5),X(6),X(7),X(8)};
|
||||
shared_nlf actRekey = init->rekey(new_keys);
|
||||
EXPECT(actRekey.get() != init.get()); // Ensure different pointers
|
||||
|
||||
|
|
|
|||
|
|
@ -153,8 +153,8 @@ int main(int argc, char *argv[]) {
|
|||
if(!isam2.getLinearizationPoint().exists(lmKey))
|
||||
{
|
||||
Pose pose = isam2.calculateEstimate<Pose>(poseKey);
|
||||
Rot2 measuredBearing = measurement->measured().first;
|
||||
double measuredRange = measurement->measured().second;
|
||||
Rot2 measuredBearing = measurement->measured().bearing();
|
||||
double measuredRange = measurement->measured().range();
|
||||
newVariables.insert(lmKey,
|
||||
pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
|
||||
}
|
||||
|
|
@ -231,7 +231,7 @@ int main(int argc, char *argv[]) {
|
|||
for (Key key2: boost::adaptors::reverse(values.keys())) {
|
||||
if(i != j) {
|
||||
gttic_(jointMarginalInformation);
|
||||
std::vector<Key> keys(2);
|
||||
KeyVector keys(2);
|
||||
keys[0] = key1;
|
||||
keys[1] = key2;
|
||||
JointMarginal info = marginals.jointMarginalInformation(keys);
|
||||
|
|
|
|||
|
|
@ -39,8 +39,8 @@ void timeAll(size_t m, size_t N) {
|
|||
// create F
|
||||
static const int D = CAMERA::dimension;
|
||||
typedef Eigen::Matrix<double, 2, D> Matrix2D;
|
||||
FastVector<Key> keys;
|
||||
vector <Matrix2D> Fblocks;
|
||||
KeyVector keys;
|
||||
vector <Matrix2D, Eigen::aligned_allocator<Matrix2D>> Fblocks;
|
||||
for (size_t i = 0; i < m; i++) {
|
||||
keys.push_back(i);
|
||||
Fblocks.push_back((i + 1) * Matrix::Ones(2, D));
|
||||
|
|
|
|||
Loading…
Reference in New Issue