Bug fix in SPQRUtils makeStairs and corresponding unit test. Copied only part of matrix when any row in A had greater than m leading zeros.
Richard Roberts
parent
d938f92844
commit
8628ef8ed2
|
|
@ -35,7 +35,7 @@ namespace gtsam {
|
|||
int j;
|
||||
vector<int> sorted;
|
||||
list<int>::iterator itRemained;
|
||||
for(j = 0; j < min(m,n); ) {
|
||||
for(j = 0; j < n; ) {
|
||||
// remove the non-zero rows in the current column
|
||||
for(itRemained = remained.begin(); itRemained!=remained.end(); ) {
|
||||
if (*(a[*itRemained]) != 0) {
|
||||
|
|
|
|||
|
|
@ -118,6 +118,72 @@ TEST(SPQRUtil, houseHolder_spqr3)
|
|||
householder_spqr(A1);
|
||||
CHECK(assert_equal(expected1, A1, 1e-3));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
TEST(SPQRUtil, houseHolder_spqr4)
|
||||
{
|
||||
Matrix A = Matrix_(15, 34,
|
||||
-5.48351, 23.2337, -45.2073, 6.33455,-0.342553,-0.897005, 7.91126, 3.20237, -2.49219, -2.44189,-0.977376, -1.61127, -3.68421,-1.28059, -2.83303, 2.45949, 0.218835, -0.71239,-0.169314,-0.131355, 2.04233, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.0782689,
|
||||
-15.8515, 10.2731, 23.2069, 0.0, 2.0891, 9.02822, 2.18705, 6.1083, -10.5157,-0.690031,-0.638592, -2.47301, -1.16601,-1.35043, -5.39516, 0.69744, -1.22492, -1.86158, -5.12896, -7.27133, -18.7928, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.360876,
|
||||
13.5817, 17.9017, 50.0056, 0.0, 0.0, -1.10618, 6.61197, -6.7864, 8.87588, -2.01464, 1.49684, 3.39016, -2.92526, 2.16326, 6.17234, 1.98881, 0.309325, 1.86023, -4.94073, 2.7695, 7.85143, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0619088,
|
||||
12.797, 4.79759, -15.866, -1.94292, 0.436084, 1.43799, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -8.05369,-0.492175, -7.84104, 1.53703,-0.807928, 0.144945,-0.341446, -2.06456, 2.259, 0.101199, 0.161626,-0.184581, -11.7786, -1.58626, 4.21069,-0.179109,
|
||||
-9.32565, 6.08188, 4.92746, 0.0,-0.0726655, -0.280519, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.5653, 10.1011, 19.53,-0.948225, -16.6125, -4.1864, 4.82523, 5.36202, 12.5239, 0.410163, 0.493983,-0.122754, -1.62954, -5.42323, 24.9016, 0.868911,
|
||||
-8.2164, -7.81388, -9.60582, 0.0, 0.0,-0.0461159, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -16.7678, 5.91287, 12.5045, 6.54954, -15.7228, -10.519, -7.66513, 0.958071,0.462615, 0.744471,-0.334807, -1.22164, -3.33139, 9.43355, -14.4208,-0.831651,
|
||||
-1.1057, 1.99291, 3.61474, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 6.56605, 4.08432, 7.3415, -4.07027, -25.9866, -19.3138, -2.7792, -3.83993,-3.53144, 0.603377,-0.588407,-0.296625, -17.2456, -9.02528, -51.079, -1.49078,
|
||||
0.0, 1.5495, 2.63487, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.63944, 4.62248, 4.4997, -29.3868, -6.10506, 20.9431, 10.654, -11.8748,-0.904113, -1.14369, 2.23985, 3.24988, 10.8288, 32.2749, 0.665805,-0.840659,
|
||||
0.0, 0.0,-0.576351, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60377, 5.9181, 5.34736, 18.6972, -1.10715, 39.0521, -5.25853, -14.9718, 4.29131, 0.480123, 3.42935, -1.58455, 24.3192, -17.98, 4.7336, 0.939854,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -6.19444,-0.142454, 0.586582, 29.6886, -4.73646, -5.11474, 39.6994, -1.12835,-3.69964, -3.04975,0.0965116, 8.58342, -3.79485, 19.0323, -5.69059, -1.11543,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.44195, -4.96618, -1.12282, -1.01802, -46.1653, 0.864773, -18.0404, 24.8898, 1.64442, 5.72634,-0.948517, 17.2222, 0.916729, -2.30198, 1.17404,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.33992, 1.16655, -18.9535, -24.3327, -8.3228, -25.6997,-42.6673, -3.59911,0.0388951, 1.07185, 7.14524, -5.94804, 28.0376,-0.364333,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.06503, -4.87522, 3.87236, -11.3562, 3.23001, 33.5579, -13.8812, 7.18718, 1.71541,-0.495603,-0.826235, -6.04699, -1.9388,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60639, -2.2334, 9.04169, 13.1342,-7.14457, 5.82756, 14.771, -49.7693, -4.22287, 2.58753, 1.22899,-0.752895,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.86199, 8.53755, -19.6873, 20.8838, 6.09985, -12.3691, -28.4341, 7.05672, 3.02888, 0.594889,-0.214789);
|
||||
|
||||
Matrix expectedR = Matrix_(15, 34,
|
||||
28.0225, 0.698206, 13.7437, -2.12682,-0.891379, -4.70403, 0.419395, -7.37109, 10.738,-0.108275, 1.27796, 3.35731,-0.037285, 2.06295, 6.5978,0.0881205, 0.799999, 2.09404, 0.0, 0.0, 0.0,-0.742297, 10.7949, 5.30571, 0.59541, -2.85665,-3.13251, -0.332376,0.0308008, 0.326458, -3.17932, -1.32946,-0.120428, 0.181167,
|
||||
0.0, 33.0569, 3.44432, 4.21511, 0.477218, 1.84355, 9.81181, 0.629595, -0.43972, -3.01942,-0.101787,-0.135997, -4.53513,-0.191803, -0.46459, 3.02053,-0.0762487, -0.116055, 0.0, 0.0, 0.0, -3.10672, -1.53799, 1.44251, 2.96932,-0.267359, 2.33355, -0.0960386, 0.262347, 0.366951, -1.68673, -2.46105, 5.55563,0.0637128,
|
||||
0.0, 0.0, -72.5995, 3.31723, -0.92697, -3.15842, 0.217843, 3.35038, -2.29212,-0.0760686, -1.19838, -1.9188,-0.128748,-1.47433, -3.06396,0.0986865, 0.462591,-0.738925, 0.0, 0.0, 0.0, 2.39657, 2.3479, 0.508439, -1.45276,-0.738523,-0.534709, 0.04058,-0.0489968, -0.230194, -2.92748,0.0364329, 0.119466,-0.0346618,
|
||||
0.0, 0.0, 0.0, 3.25682, -1.182, -4.84344, 2.74062, -2.81233, 5.06869,-0.834871, 0.28589, 1.18891, -1.18948,0.606334, 2.52042, 0.831464, 0.575576, 0.884713, 4.47525,0.0381282, 8.65006, 0.178133, 7.13055, 0.99353, -1.7708, 0.464406,-5.86884, -0.194461,-0.365941,0.0828452, 10.1153, 3.22621, -9.90261,0.0257706,
|
||||
0.0, 0.0, 0.0, 0.0, 1.18256, 5.14989, 0.838654, 3.86831, -6.31407,-0.268897,-0.494264, -1.63226,-0.480456,-0.931946, -3.4322, 0.275576,-0.655629, -1.15196, -7.78905, -13.5715, -29.2364, 3.37589, 18.448, 5.11948, -4.08059, -3.2509,-9.52461, -0.362224,-0.457579,-0.0601673, 1.85657, 2.99257, -12.1144,-0.624855,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.450726, -2.52457, 2.18784, -3.04253, 0.770326,-0.443888, -1.07379, 1.12148,-0.662977, -1.98947,-0.762824,-0.151537,-0.615483, 19.9937, -5.17055, -11.2101, -10.0805, 10.6168, 9.36274, 8.17588, -1.78258,-0.858858, -0.75124, 0.443364, 1.48335, 4.46589, -5.72814, 8.27179, 0.551859,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0812029,-0.0832869, 0.0674935,-0.0247479, 0.0283366,0.0480445,-0.036613,0.0351956, 0.0768672,0.0242134,-0.00953749,0.0194382, -5.93603, -5.0025, -6.38014, 18.5158, 22.668, 1.61251, -1.86948, 11.5217, 5.39137, 0.160562,-0.866767, -1.46548, 6.35692, -13.7392, 45.5091, 1.89557,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.0237536, 0.0206287,6.48832e-05,0.00746251, 0.0129591,0.000272306,0.00955241, 0.0216226,-0.000202751, -0.00189634,0.00560094, -5.77613, 5.44125, 4.94628, 21.3185,-0.976758, 36.3015, -6.24453, -13.7772, 4.2347, 0.597408, 3.16863, -1.89053, 22.7518, -21.5891, 3.36502, 0.993638,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.00807513,-1.07967e-06, 0.00194212, 0.00125444, -0.000133756, 0.00168498, 0.000217439,-4.13036e-05, -0.00259827,-0.000661005, 1.19994, 2.30552, 0.746869, -18.6973, 9.7233, 31.6093, 9.52016, -8.27898, 2.32924, -1.18233, 2.47028, 2.54466, 21.2909, 29.1971, 32.4215, 0.342241,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -6.19444,-0.142454, 0.586582, 29.6886, -4.73646, -5.11474, 39.6994, -1.12835,-3.69964, -3.04975,0.0965116, 8.58342, -3.79485, 19.0323, -5.69059, -1.11543,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.44195, -4.96618, -1.12282, -1.01802, -46.1653, 0.864773, -18.0404, 24.8898, 1.64442, 5.72634,-0.948517, 17.2222, 0.916729, -2.30198, 1.17404,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.33992, 1.16655, -18.9535, -24.3327, -8.3228, -25.6997,-42.6673, -3.59911,0.0388951, 1.07185, 7.14524, -5.94804, 28.0376,-0.364333,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.06503, -4.87522, 3.87236, -11.3562, 3.23001, 33.5579, -13.8812, 7.18718, 1.71541,-0.495603,-0.826235, -6.04699, -1.9388,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60639, -2.2334, 9.04169, 13.1342,-7.14457, 5.82756, 14.771, -49.7693, -4.22287, 2.58753, 1.22899,-0.752895,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.86199, 8.53755, -19.6873, 20.8838, 6.09985, -12.3691, -28.4341, 7.05672, 3.02888, 0.594889,-0.214789);
|
||||
|
||||
Matrix expectedA = Matrix_(15, 34,
|
||||
-5.48351, 23.2337, -45.2073, 6.33455,-0.342553,-0.897005, 7.91126, 3.20237, -2.49219, -2.44189,-0.977376, -1.61127, -3.68421,-1.28059, -2.83303, 2.45949, 0.218835, -0.71239,-0.169314,-0.131355, 2.04233, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.0782689,
|
||||
-15.8515, 10.2731, 23.2069, 0.0, 2.0891, 9.02822, 2.18705, 6.1083, -10.5157,-0.690031,-0.638592, -2.47301, -1.16601,-1.35043, -5.39516, 0.69744, -1.22492, -1.86158, -5.12896, -7.27133, -18.7928, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.360876,
|
||||
13.5817, 17.9017, 50.0056, 0.0, 0.0, -1.10618, 6.61197, -6.7864, 8.87588, -2.01464, 1.49684, 3.39016, -2.92526, 2.16326, 6.17234, 1.98881, 0.309325, 1.86023, -4.94073, 2.7695, 7.85143, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0619088,
|
||||
12.797, 4.79759, -15.866, -1.94292, 0.436084, 1.43799, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -8.05369,-0.492175, -7.84104, 1.53703,-0.807928, 0.144945,-0.341446, -2.06456, 2.259, 0.101199, 0.161626,-0.184581, -11.7786, -1.58626, 4.21069,-0.179109,
|
||||
-9.32565, 6.08188, 4.92746, 0.0,-0.0726655, -0.280519, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.5653, 10.1011, 19.53,-0.948225, -16.6125, -4.1864, 4.82523, 5.36202, 12.5239, 0.410163, 0.493983,-0.122754, -1.62954, -5.42323, 24.9016, 0.868911,
|
||||
-8.2164, -7.81388, -9.60582, 0.0, 0.0,-0.0461159, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -16.7678, 5.91287, 12.5045, 6.54954, -15.7228, -10.519, -7.66513, 0.958071,0.462615, 0.744471,-0.334807, -1.22164, -3.33139, 9.43355, -14.4208,-0.831651,
|
||||
-1.1057, 1.99291, 3.61474, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 6.56605, 4.08432, 7.3415, -4.07027, -25.9866, -19.3138, -2.7792, -3.83993,-3.53144, 0.603377,-0.588407,-0.296625, -17.2456, -9.02528, -51.079, -1.49078,
|
||||
0.0, 1.5495, 2.63487, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.63944, 4.62248, 4.4997, -29.3868, -6.10506, 20.9431, 10.654, -11.8748,-0.904113, -1.14369, 2.23985, 3.24988, 10.8288, 32.2749, 0.665805,-0.840659,
|
||||
0.0, 0.0,-0.576351, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60377, 5.9181, 5.34736, 18.6972, -1.10715, 39.0521, -5.25853, -14.9718, 4.29131, 0.480123, 3.42935, -1.58455, 24.3192, -17.98, 4.7336, 0.939854,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -6.19444,-0.142454, 0.586582, 29.6886, -4.73646, -5.11474, 39.6994, -1.12835,-3.69964, -3.04975,0.0965116, 8.58342, -3.79485, 19.0323, -5.69059, -1.11543,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.44195, -4.96618, -1.12282, -1.01802, -46.1653, 0.864773, -18.0404, 24.8898, 1.64442, 5.72634,-0.948517, 17.2222, 0.916729, -2.30198, 1.17404,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.33992, 1.16655, -18.9535, -24.3327, -8.3228, -25.6997,-42.6673, -3.59911,0.0388951, 1.07185, 7.14524, -5.94804, 28.0376,-0.364333,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.06503, -4.87522, 3.87236, -11.3562, 3.23001, 33.5579, -13.8812, 7.18718, 1.71541,-0.495603,-0.826235, -6.04699, -1.9388,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60639, -2.2334, 9.04169, 13.1342,-7.14457, 5.82756, 14.771, -49.7693, -4.22287, 2.58753, 1.22899,-0.752895,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.86199, 8.53755, -19.6873, 20.8838, 6.09985, -12.3691, -28.4341, 7.05672, 3.02888, 0.594889,-0.214789);
|
||||
|
||||
Matrix actualR = A;
|
||||
householder_spqr(actualR);
|
||||
|
||||
long* Stair = MakeStairs(A);
|
||||
CHECK(assert_equal(expectedA, A));
|
||||
|
||||
Matrix actualRstair = A;
|
||||
householder_spqr(actualRstair, Stair);
|
||||
|
||||
CHECK(assert_equal(expectedR, actualR, 1e-3));
|
||||
CHECK(assert_equal(expectedR, actualRstair, 1e-3));
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue