29 using namespace gtsam;
31 static double inf = std::numeric_limits<double>::infinity();
32 static const double tol = 1
e-9;
64 template<
typename Derived>
71 template<
typename Derived>
91 Matrix actual3 = (
Matrix(2,2) << 1, 2, submat1).finished();
94 Matrix actual4 = (
Matrix(2,2) << submat2, 3, 4).finished();
96 Matrix actual5 = testFcn1((
Matrix(2,2) << 1, 2, 3, 4).finished());
97 Matrix actual6 = testFcn2((
Matrix(2,2) << 1, 2, 3, 4).finished());
111 const double *
const a = &
A(0, 0);
121 Matrix A = (
Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
122 Matrix B = (
Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
125 for (
int i = 0;
i < 2;
i++)
126 for (
int j = 0;
j < 2;
j++)
128 for (
int i = 0;
i < 2;
i++)
129 for (
int j = 0;
j < 3;
j++)
138 Matrix A = (
Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
139 Matrix B = (
Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
140 vector<const Matrix*> matrices;
141 matrices.push_back(&
A);
142 matrices.push_back(&
B);
145 for (
int i = 0;
i < 2;
i++)
146 for (
int j = 0;
j < 2;
j++)
148 for (
int i = 0;
i < 2;
i++)
149 for (
int j = 0;
j < 3;
j++)
159 A = Matrix::Identity(2,3);
160 B = Matrix::Identity(2,3);
161 vector<const Matrix*> matrices;
162 matrices.push_back(&
A);
163 matrices.push_back(&
B);
166 1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
167 0.0, 1.0, 0.0, 0.0, 1.0, 0.0).finished();
175 Matrix A = (
Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
176 Matrix B = (
Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
179 for (
int i = 0;
i < 2;
i++)
180 for (
int j = 0;
j < 2;
j++)
182 for (
int i = 0;
i < 3;
i++)
183 for (
int j = 0;
j < 2;
j++)
188 std::vector<gtsam::Matrix> matrices;
189 matrices.push_back(
A);
190 matrices.push_back(
B);
198 Matrix A = (
Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
199 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1.,
202 Vector exp1 = (
Vector(4) << -1., 0., 1., 0.).finished();
210 Vector exp3 = (
Vector(4) << -0.2, 0.3, 0.2, -0.1).finished();
217 Matrix A = (
Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
218 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1.,
221 Vector exp1 = (
Vector(7) << -1., 0., 1., 0., 0., 0., -0.2).finished();
229 Vector exp3 = (
Vector(7) << 0., 1., 0., 0., 0., -1., -0.1).finished();
236 Matrix big = Matrix::Zero(5,6), small = (
Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
242 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0,
243 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished();
251 std::vector<Matrix> Hs;
252 Hs.push_back(Matrix::Ones(3,3));
253 Hs.push_back(Matrix::Ones(4,4)*2);
254 Hs.push_back(Matrix::Ones(2,2)*3);
259 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
260 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
261 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
262 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
263 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
264 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
265 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
266 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0,
267 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0).finished();
276 -0.3, -8
e-2, 5.1, 9.0,
277 1.2, 3.4, 4.5, 6.7).finished();
279 string matrixAsString =
281 "-0.3 -8e-2 5.1 9.0\n\r"
282 "1.2 \t 3.4 4.5 6.7";
284 stringstream asStream(matrixAsString, ios::in);
385 Vector v = (
Vector(3) << 2., std::numeric_limits<double>::infinity(), 4.).finished();
409 double wx = 1, wy = 2, wz = 3;
505 Matrix C = (
Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0).finished();
514 Matrix C = (
Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0).finished();
564 Matrix A = (
Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
604 1.0, 2.0, 3.0, 4.0).finished();
609 0.0, 0.0, 3.0, 4.0).finished();
627 1.0, 2.0, 3.0).finished();
633 0.0, 0.0, 0.0).finished();
648 0.0, 2.0, 3.0).finished();
686 Matrix lMg((
Matrix(3, 3) << 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0).finished());
690 0.0, 0.0, 1.0).finished(),
692 Matrix gMl((
Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished());
696 0.0, 0.0, 1.0).finished(),
735 Matrix U22 = (
Matrix(2, 2) << 2., 3., 0., 4.).finished();
741 Matrix U33 = (
Matrix(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.).finished();
760 Matrix expected1 = (
Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236,
761 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565,
762 -0.618034, 0, 4.4721, 0, -4.4721, 0, 0,
763 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894).finished();
765 00,-5, 0, 5, 0, 0, 1.5,
766 10, 0, 0, 0,-10,0, 2,
767 00, 10,0, 0, 0, -10, -1 ).finished();
773 Matrix expected = (
Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
774 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0,
775 0, 0, 4.4721, 0, -4.4721, 0.894).finished();
777 00,-5, 0, 5, 0, 0, 1.5,
778 10, 0, 0, 0,-10,0, 2,
779 00, 10,0, 0, 0, -10, -1).finished();
789 Matrix expected1((
Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236,
790 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565,
791 -0.618034, 0, 4.4721, 0, -4.4721, 0, 0,
792 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894).finished());
794 00,-5, 0, 5, 0, 0, 1.5,
795 10, 0, 0, 0,-10,0, 2,
796 00, 10,0, 0, 0, -10, -1).finished());
802 Matrix expected((
Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
803 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0,
804 0, 0, 4.4721, 0, -4.4721, 0.894).finished());
806 00,-5, 0, 5, 0, 0, 1.5,
807 10, 0, 0, 0,-10,0, 2,
808 00, 10,0, 0, 0, -10, -1).finished());
820 Matrix expected((
Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
821 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0,
822 0, 0, 4.4721, 0, -4.4721, 0.894).finished());
824 00,-5, 0, 5, 0, 0, 1.5,
825 10, 0, 0, 0,-10,0, 2,
826 00, 10,0, 0, 0, -10, -1).finished());
827 Matrix actual =
A.householderQr().matrixQR();
834 00,-5, 0, 5, 0, 0, 1.5,
835 10, 0, 0, 0,-10,0, 2,
836 00, 10,0, 0, 0, -10, -1).finished());
848 Matrix A = (
Matrix(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00,
849 0, 0, -10, 10, 0, -10, 0).finished();
852 Matrix expectedQ = (
Matrix(6, 6) << -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0,
853 0.3651, -0.8165, 0, 00.6667, 0, 0.7454, 0, 0, 0, 0000000, 0.8944,
854 0, 0.1826, -0.4082, 0, 0000000, 0, 0, -0.9129, -0.4082, 0, 00.6667,
855 0, -0.5963, 0, 0, -0.4472).finished();
857 Matrix expectedR = (
Matrix(6, 4) << 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0,
858 7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0).finished();
860 const auto [
Q,
R] =
qr(
A);
869 Matrix A = (
Matrix(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10,
870 0, 00, 10, 0, 0, 0, -10).finished();
890 const double*
a = &
A(0, 0);
898 Matrix A = (
Matrix(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0.,
899 1., 0., 0., 0., -1., 0., 0., 1., 0., 0., 0., -1.).finished();
904 Matrix expectedR = (
Matrix(4, 6) << 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0.,
905 -0.2, 0., -0.8, 0., 0., 1., 0., -1., 0., 0., 0., 0., 1., 0., -1.).finished();
907 Vector newSigmas = (
Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished();
912 std::list<std::tuple<Vector, double, double> > solution =
917 for (
const auto& [r, di,
sigma] : solution) {
928 Matrix measurement_covariance = (
Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25,
929 0.0, 0.0, 0.0, 0.01).finished();
945 0.0785892, 0.0137923, -0.0142219, -0.0171880, 0.0028726,
946 0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868,
947 -0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064,
948 -0.0171880,-0.0101952, 0.0054906, 0.0892453, -0.0059468,
949 0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517).finished();
952 3.567126953241796, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
953 -0.590030436566913, 3.362022286742925, 0.000000000000000, 0.000000000000000, 0.000000000000000,
954 0.618207860252376, -0.168166020746503, 3.253086082942785, 0.000000000000000, 0.000000000000000,
955 0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000,
956 -0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937).finished();
966 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363,
967 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463,
968 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363,
969 -0.0021113, 0.0036415, 0.0909464).finished();
972 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
973 -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000,
974 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000,
975 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000,
976 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247).finished();
995 Matrix A = (
Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
996 Matrix B = (
Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished();
1003 Matrix A = (
Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
1004 Matrix B = (
Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished();
1011 Matrix A = (
Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
1012 Matrix B = (
Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0).finished();
1020 Matrix U1 = Matrix::Identity(4, 3),
S1 =
v.asDiagonal(), V1 = I_3x3,
A = (U1 *
S1)
1041 Matrix expectedU = (
Matrix(3, 2) << 0.,-1.,0.,0.,1.,0.).finished();
1043 Matrix expectedV = (
Matrix(2, 2) << 1.,0.,0.,1.).finished();
1064 Matrix expectedU = (
Matrix(2, 2) << -1.,0.,0.,-1.).finished();
1066 Matrix expectedV = (
Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.).finished();
1095 0.1270, 0.0975).finished();
1100 0.0970, -0.0689).finished();
1106 0.6723, 0.7403).finished();
1112 U.col(0) = -
U.col(0);
1113 V.col(0) = -
V.col(0);
1116 U.col(1) = -
U.col(1);
1117 V.col(1) = -
V.col(1);
1132 0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11,
1133 0.44, -0.66, -15.84, 0.34, -0.51, -12.24, -1.64, 2.46, 59.04,
1134 0.69, -8.28, -12.19, -0.48, 5.76, 8.48, -1.89, 22.68, 33.39,
1135 0.96, -8.4, -17.76, -0.6, 5.25, 11.1, -3.36, 29.4, 62.16,
1136 1.25, 0.3, 2.75, -3.5, -0.84, -7.7, 16.25, 3.9, 35.75,
1137 1.56, 0.42, 4.56, -3.38, -0.91, -9.88, 22.36, 6.02, 65.36,
1138 1.89, 2.24, 3.99, 3.24, 3.84, 6.84, 18.09, 21.44, 38.19,
1139 2.24, 2.48, 6.24, 3.08, 3.41, 8.58, 24.64, 27.28, 68.64
1141 const auto [rank,
error,actual] =
DLT(
A);
1142 Vector expected = (
Vector(9) << -0.0, 0.2357, 0.4714, -0.2357, 0.0, - 0.4714,-0.4714, 0.4714, 0.0).finished();
1154 #ifdef GTSAM_USE_BOOST_FEATURES
1169 #ifdef GTSAM_USE_BOOST_FEATURES
1178 double a = 2000,
b = 1997,
tol = 1
e-1;
1193 Matrix A = Matrix::Random(3, 3);
1194 Matrix B = Matrix::Random(3, 3);