1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are met: 7 // 8 // * Redistributions of source code must retain the above copyright notice, 9 // this list of conditions and the following disclaimer. 10 // * Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // * Neither the name of Google Inc. nor the names of its contributors may be 14 // used to endorse or promote products derived from this software without 15 // specific prior written permission. 16 // 17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 // POSSIBILITY OF SUCH DAMAGE. 28 // 29 // Author: sameeragarwal (at) google.com (Sameer Agarwal) 30 31 #include "ceres/compressed_row_sparse_matrix.h" 32 33 #include <numeric> 34 #include "ceres/casts.h" 35 #include "ceres/crs_matrix.h" 36 #include "ceres/cxsparse.h" 37 #include "ceres/internal/eigen.h" 38 #include "ceres/internal/scoped_ptr.h" 39 #include "ceres/linear_least_squares_problems.h" 40 #include "ceres/random.h" 41 #include "ceres/triplet_sparse_matrix.h" 42 #include "glog/logging.h" 43 #include "gtest/gtest.h" 44 45 namespace ceres { 46 namespace internal { 47 48 void CompareMatrices(const SparseMatrix* a, const SparseMatrix* b) { 49 EXPECT_EQ(a->num_rows(), b->num_rows()); 50 EXPECT_EQ(a->num_cols(), b->num_cols()); 51 52 int num_rows = a->num_rows(); 53 int num_cols = a->num_cols(); 54 55 for (int i = 0; i < num_cols; ++i) { 56 Vector x = Vector::Zero(num_cols); 57 x(i) = 1.0; 58 59 Vector y_a = Vector::Zero(num_rows); 60 Vector y_b = Vector::Zero(num_rows); 61 62 a->RightMultiply(x.data(), y_a.data()); 63 b->RightMultiply(x.data(), y_b.data()); 64 65 EXPECT_EQ((y_a - y_b).norm(), 0); 66 } 67 } 68 69 class CompressedRowSparseMatrixTest : public ::testing::Test { 70 protected : 71 virtual void SetUp() { 72 scoped_ptr<LinearLeastSquaresProblem> problem( 73 CreateLinearLeastSquaresProblemFromId(1)); 74 75 CHECK_NOTNULL(problem.get()); 76 77 tsm.reset(down_cast<TripletSparseMatrix*>(problem->A.release())); 78 crsm.reset(new CompressedRowSparseMatrix(*tsm)); 79 80 num_rows = tsm->num_rows(); 81 num_cols = tsm->num_cols(); 82 83 vector<int>* row_blocks = crsm->mutable_row_blocks(); 84 row_blocks->resize(num_rows); 85 std::fill(row_blocks->begin(), row_blocks->end(), 1); 86 87 vector<int>* col_blocks = crsm->mutable_col_blocks(); 88 col_blocks->resize(num_cols); 89 std::fill(col_blocks->begin(), col_blocks->end(), 1); 90 } 91 92 int num_rows; 93 int num_cols; 94 95 scoped_ptr<TripletSparseMatrix> tsm; 96 scoped_ptr<CompressedRowSparseMatrix> crsm; 97 }; 98 99 TEST_F(CompressedRowSparseMatrixTest, RightMultiply) { 100 CompareMatrices(tsm.get(), crsm.get()); 101 } 102 103 TEST_F(CompressedRowSparseMatrixTest, LeftMultiply) { 104 for (int i = 0; i < num_rows; ++i) { 105 Vector a = Vector::Zero(num_rows); 106 a(i) = 1.0; 107 108 Vector b1 = Vector::Zero(num_cols); 109 Vector b2 = Vector::Zero(num_cols); 110 111 tsm->LeftMultiply(a.data(), b1.data()); 112 crsm->LeftMultiply(a.data(), b2.data()); 113 114 EXPECT_EQ((b1 - b2).norm(), 0); 115 } 116 } 117 118 TEST_F(CompressedRowSparseMatrixTest, ColumnNorm) { 119 Vector b1 = Vector::Zero(num_cols); 120 Vector b2 = Vector::Zero(num_cols); 121 122 tsm->SquaredColumnNorm(b1.data()); 123 crsm->SquaredColumnNorm(b2.data()); 124 125 EXPECT_EQ((b1 - b2).norm(), 0); 126 } 127 128 TEST_F(CompressedRowSparseMatrixTest, Scale) { 129 Vector scale(num_cols); 130 for (int i = 0; i < num_cols; ++i) { 131 scale(i) = i + 1; 132 } 133 134 tsm->ScaleColumns(scale.data()); 135 crsm->ScaleColumns(scale.data()); 136 CompareMatrices(tsm.get(), crsm.get()); 137 } 138 139 TEST_F(CompressedRowSparseMatrixTest, DeleteRows) { 140 // Clear the row and column blocks as these are purely scalar tests. 141 crsm->mutable_row_blocks()->clear(); 142 crsm->mutable_col_blocks()->clear(); 143 for (int i = 0; i < num_rows; ++i) { 144 tsm->Resize(num_rows - i, num_cols); 145 crsm->DeleteRows(crsm->num_rows() - tsm->num_rows()); 146 CompareMatrices(tsm.get(), crsm.get()); 147 } 148 } 149 150 TEST_F(CompressedRowSparseMatrixTest, AppendRows) { 151 // Clear the row and column blocks as these are purely scalar tests. 152 crsm->mutable_row_blocks()->clear(); 153 crsm->mutable_col_blocks()->clear(); 154 155 for (int i = 0; i < num_rows; ++i) { 156 TripletSparseMatrix tsm_appendage(*tsm); 157 tsm_appendage.Resize(i, num_cols); 158 159 tsm->AppendRows(tsm_appendage); 160 CompressedRowSparseMatrix crsm_appendage(tsm_appendage); 161 crsm->AppendRows(crsm_appendage); 162 163 CompareMatrices(tsm.get(), crsm.get()); 164 } 165 } 166 167 TEST_F(CompressedRowSparseMatrixTest, AppendAndDeleteBlockDiagonalMatrix) { 168 int num_diagonal_rows = crsm->num_cols(); 169 170 scoped_array<double> diagonal(new double[num_diagonal_rows]); 171 for (int i = 0; i < num_diagonal_rows; ++i) { 172 diagonal[i] =i; 173 } 174 175 vector<int> row_and_column_blocks; 176 row_and_column_blocks.push_back(1); 177 row_and_column_blocks.push_back(2); 178 row_and_column_blocks.push_back(2); 179 180 const vector<int> pre_row_blocks = crsm->row_blocks(); 181 const vector<int> pre_col_blocks = crsm->col_blocks(); 182 183 scoped_ptr<CompressedRowSparseMatrix> appendage( 184 CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( 185 diagonal.get(), row_and_column_blocks)); 186 LOG(INFO) << appendage->row_blocks().size(); 187 188 crsm->AppendRows(*appendage); 189 190 const vector<int> post_row_blocks = crsm->row_blocks(); 191 const vector<int> post_col_blocks = crsm->col_blocks(); 192 193 vector<int> expected_row_blocks = pre_row_blocks; 194 expected_row_blocks.insert(expected_row_blocks.end(), 195 row_and_column_blocks.begin(), 196 row_and_column_blocks.end()); 197 198 vector<int> expected_col_blocks = pre_col_blocks; 199 200 EXPECT_EQ(expected_row_blocks, crsm->row_blocks()); 201 EXPECT_EQ(expected_col_blocks, crsm->col_blocks()); 202 203 crsm->DeleteRows(num_diagonal_rows); 204 EXPECT_EQ(crsm->row_blocks(), pre_row_blocks); 205 EXPECT_EQ(crsm->col_blocks(), pre_col_blocks); 206 } 207 208 TEST_F(CompressedRowSparseMatrixTest, ToDenseMatrix) { 209 Matrix tsm_dense; 210 Matrix crsm_dense; 211 212 tsm->ToDenseMatrix(&tsm_dense); 213 crsm->ToDenseMatrix(&crsm_dense); 214 215 EXPECT_EQ((tsm_dense - crsm_dense).norm(), 0.0); 216 } 217 218 TEST_F(CompressedRowSparseMatrixTest, ToCRSMatrix) { 219 CRSMatrix crs_matrix; 220 crsm->ToCRSMatrix(&crs_matrix); 221 EXPECT_EQ(crsm->num_rows(), crs_matrix.num_rows); 222 EXPECT_EQ(crsm->num_cols(), crs_matrix.num_cols); 223 EXPECT_EQ(crsm->num_rows() + 1, crs_matrix.rows.size()); 224 EXPECT_EQ(crsm->num_nonzeros(), crs_matrix.cols.size()); 225 EXPECT_EQ(crsm->num_nonzeros(), crs_matrix.values.size()); 226 227 for (int i = 0; i < crsm->num_rows() + 1; ++i) { 228 EXPECT_EQ(crsm->rows()[i], crs_matrix.rows[i]); 229 } 230 231 for (int i = 0; i < crsm->num_nonzeros(); ++i) { 232 EXPECT_EQ(crsm->cols()[i], crs_matrix.cols[i]); 233 EXPECT_EQ(crsm->values()[i], crs_matrix.values[i]); 234 } 235 } 236 237 TEST(CompressedRowSparseMatrix, CreateBlockDiagonalMatrix) { 238 vector<int> blocks; 239 blocks.push_back(1); 240 blocks.push_back(2); 241 blocks.push_back(2); 242 243 Vector diagonal(5); 244 for (int i = 0; i < 5; ++i) { 245 diagonal(i) = i + 1; 246 } 247 248 scoped_ptr<CompressedRowSparseMatrix> matrix( 249 CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( 250 diagonal.data(), blocks)); 251 252 EXPECT_EQ(matrix->num_rows(), 5); 253 EXPECT_EQ(matrix->num_cols(), 5); 254 EXPECT_EQ(matrix->num_nonzeros(), 9); 255 EXPECT_EQ(blocks, matrix->row_blocks()); 256 EXPECT_EQ(blocks, matrix->col_blocks()); 257 258 Vector x(5); 259 Vector y(5); 260 261 x.setOnes(); 262 y.setZero(); 263 matrix->RightMultiply(x.data(), y.data()); 264 for (int i = 0; i < diagonal.size(); ++i) { 265 EXPECT_EQ(y[i], diagonal[i]); 266 } 267 268 y.setZero(); 269 matrix->LeftMultiply(x.data(), y.data()); 270 for (int i = 0; i < diagonal.size(); ++i) { 271 EXPECT_EQ(y[i], diagonal[i]); 272 } 273 274 Matrix dense; 275 matrix->ToDenseMatrix(&dense); 276 EXPECT_EQ((dense.diagonal() - diagonal).norm(), 0.0); 277 } 278 279 class SolveLowerTriangularTest : public ::testing::Test { 280 protected: 281 void SetUp() { 282 matrix_.reset(new CompressedRowSparseMatrix(4, 4, 7)); 283 int* rows = matrix_->mutable_rows(); 284 int* cols = matrix_->mutable_cols(); 285 double* values = matrix_->mutable_values(); 286 287 rows[0] = 0; 288 cols[0] = 0; 289 values[0] = 0.50754; 290 291 rows[1] = 1; 292 cols[1] = 1; 293 values[1] = 0.80483; 294 295 rows[2] = 2; 296 cols[2] = 1; 297 values[2] = 0.14120; 298 cols[3] = 2; 299 values[3] = 0.3; 300 301 rows[3] = 4; 302 cols[4] = 0; 303 values[4] = 0.77696; 304 cols[5] = 1; 305 values[5] = 0.41860; 306 cols[6] = 3; 307 values[6] = 0.88979; 308 309 rows[4] = 7; 310 } 311 312 scoped_ptr<CompressedRowSparseMatrix> matrix_; 313 }; 314 315 TEST_F(SolveLowerTriangularTest, SolveInPlace) { 316 double rhs_and_solution[] = {1.0, 1.0, 2.0, 2.0}; 317 double expected[] = {1.970288, 1.242498, 6.081864, -0.057255}; 318 matrix_->SolveLowerTriangularInPlace(rhs_and_solution); 319 for (int i = 0; i < 4; ++i) { 320 EXPECT_NEAR(rhs_and_solution[i], expected[i], 1e-4) << i; 321 } 322 } 323 324 TEST_F(SolveLowerTriangularTest, TransposeSolveInPlace) { 325 double rhs_and_solution[] = {1.0, 1.0, 2.0, 2.0}; 326 const double expected[] = { -1.4706, -1.0962, 6.6667, 2.2477}; 327 328 matrix_->SolveLowerTriangularTransposeInPlace(rhs_and_solution); 329 for (int i = 0; i < 4; ++i) { 330 EXPECT_NEAR(rhs_and_solution[i], expected[i], 1e-4) << i; 331 } 332 } 333 334 TEST(CompressedRowSparseMatrix, Transpose) { 335 // 0 1 0 2 3 0 336 // 4 6 7 0 0 8 337 // 9 10 0 11 12 0 338 // 13 0 14 15 9 0 339 // 0 16 17 0 0 0 340 341 // Block structure: 342 // A A A A B B 343 // A A A A B B 344 // A A A A B B 345 // C C C C D D 346 // C C C C D D 347 // C C C C D D 348 349 CompressedRowSparseMatrix matrix(5, 6, 30); 350 int* rows = matrix.mutable_rows(); 351 int* cols = matrix.mutable_cols(); 352 double* values = matrix.mutable_values(); 353 matrix.mutable_row_blocks()->push_back(3); 354 matrix.mutable_row_blocks()->push_back(3); 355 matrix.mutable_col_blocks()->push_back(4); 356 matrix.mutable_col_blocks()->push_back(2); 357 358 rows[0] = 0; 359 cols[0] = 1; 360 cols[1] = 3; 361 cols[2] = 4; 362 363 rows[1] = 3; 364 cols[3] = 0; 365 cols[4] = 1; 366 cols[5] = 2; 367 cols[6] = 5; 368 369 370 rows[2] = 7; 371 cols[7] = 0; 372 cols[8] = 1; 373 cols[9] = 3; 374 cols[10] = 4; 375 376 rows[3] = 11; 377 cols[11] = 0; 378 cols[12] = 2; 379 cols[13] = 3; 380 cols[14] = 4; 381 382 rows[4] = 15; 383 cols[15] = 1; 384 cols[16] = 2; 385 rows[5] = 17; 386 387 copy(values, values + 17, cols); 388 389 scoped_ptr<CompressedRowSparseMatrix> transpose(matrix.Transpose()); 390 391 ASSERT_EQ(transpose->row_blocks().size(), matrix.col_blocks().size()); 392 for (int i = 0; i < transpose->row_blocks().size(); ++i) { 393 EXPECT_EQ(transpose->row_blocks()[i], matrix.col_blocks()[i]); 394 } 395 396 ASSERT_EQ(transpose->col_blocks().size(), matrix.row_blocks().size()); 397 for (int i = 0; i < transpose->col_blocks().size(); ++i) { 398 EXPECT_EQ(transpose->col_blocks()[i], matrix.row_blocks()[i]); 399 } 400 401 Matrix dense_matrix; 402 matrix.ToDenseMatrix(&dense_matrix); 403 404 Matrix dense_transpose; 405 transpose->ToDenseMatrix(&dense_transpose); 406 EXPECT_NEAR((dense_matrix - dense_transpose.transpose()).norm(), 0.0, 1e-14); 407 } 408 409 #ifndef CERES_NO_CXSPARSE 410 411 struct RandomMatrixOptions { 412 int num_row_blocks; 413 int min_row_block_size; 414 int max_row_block_size; 415 int num_col_blocks; 416 int min_col_block_size; 417 int max_col_block_size; 418 double block_density; 419 }; 420 421 CompressedRowSparseMatrix* CreateRandomCompressedRowSparseMatrix( 422 const RandomMatrixOptions& options) { 423 vector<int> row_blocks; 424 for (int i = 0; i < options.num_row_blocks; ++i) { 425 const int delta_block_size = 426 Uniform(options.max_row_block_size - options.min_row_block_size); 427 row_blocks.push_back(options.min_row_block_size + delta_block_size); 428 } 429 430 vector<int> col_blocks; 431 for (int i = 0; i < options.num_col_blocks; ++i) { 432 const int delta_block_size = 433 Uniform(options.max_col_block_size - options.min_col_block_size); 434 col_blocks.push_back(options.min_col_block_size + delta_block_size); 435 } 436 437 vector<int> rows; 438 vector<int> cols; 439 vector<double> values; 440 441 while (values.size() == 0) { 442 int row_block_begin = 0; 443 for (int r = 0; r < options.num_row_blocks; ++r) { 444 int col_block_begin = 0; 445 for (int c = 0; c < options.num_col_blocks; ++c) { 446 if (RandDouble() <= options.block_density) { 447 for (int i = 0; i < row_blocks[r]; ++i) { 448 for (int j = 0; j < col_blocks[c]; ++j) { 449 rows.push_back(row_block_begin + i); 450 cols.push_back(col_block_begin + j); 451 values.push_back(RandNormal()); 452 } 453 } 454 } 455 col_block_begin += col_blocks[c]; 456 } 457 row_block_begin += row_blocks[r]; 458 } 459 } 460 461 const int num_rows = std::accumulate(row_blocks.begin(), row_blocks.end(), 0); 462 const int num_cols = std::accumulate(col_blocks.begin(), col_blocks.end(), 0); 463 const int num_nonzeros = values.size(); 464 465 TripletSparseMatrix tsm(num_rows, num_cols, num_nonzeros); 466 std::copy(rows.begin(), rows.end(), tsm.mutable_rows()); 467 std::copy(cols.begin(), cols.end(), tsm.mutable_cols()); 468 std::copy(values.begin(), values.end(), tsm.mutable_values()); 469 tsm.set_num_nonzeros(num_nonzeros); 470 CompressedRowSparseMatrix* matrix = new CompressedRowSparseMatrix(tsm); 471 (*matrix->mutable_row_blocks()) = row_blocks; 472 (*matrix->mutable_col_blocks()) = col_blocks; 473 return matrix; 474 } 475 476 void ToDenseMatrix(const cs_di* matrix, Matrix* dense_matrix) { 477 dense_matrix->resize(matrix->m, matrix->n); 478 dense_matrix->setZero(); 479 480 for (int c = 0; c < matrix->n; ++c) { 481 for (int idx = matrix->p[c]; idx < matrix->p[c + 1]; ++idx) { 482 const int r = matrix->i[idx]; 483 (*dense_matrix)(r, c) = matrix->x[idx]; 484 } 485 } 486 } 487 488 TEST(CompressedRowSparseMatrix, ComputeOuterProduct) { 489 // "Randomly generated seed." 490 SetRandomState(29823); 491 int kMaxNumRowBlocks = 10; 492 int kMaxNumColBlocks = 10; 493 int kNumTrials = 10; 494 495 CXSparse cxsparse; 496 const double kTolerance = 1e-18; 497 498 // Create a random matrix, compute its outer product using CXSParse 499 // and ComputeOuterProduct. Convert both matrices to dense matrices 500 // and compare their upper triangular parts. They should be within 501 // kTolerance of each other. 502 for (int num_row_blocks = 1; 503 num_row_blocks < kMaxNumRowBlocks; 504 ++num_row_blocks) { 505 for (int num_col_blocks = 1; 506 num_col_blocks < kMaxNumColBlocks; 507 ++num_col_blocks) { 508 for (int trial = 0; trial < kNumTrials; ++trial) { 509 510 511 RandomMatrixOptions options; 512 options.num_row_blocks = num_row_blocks; 513 options.num_col_blocks = num_col_blocks; 514 options.min_row_block_size = 1; 515 options.max_row_block_size = 5; 516 options.min_col_block_size = 1; 517 options.max_col_block_size = 10; 518 options.block_density = std::max(0.1, RandDouble()); 519 520 VLOG(2) << "num row blocks: " << options.num_row_blocks; 521 VLOG(2) << "num col blocks: " << options.num_col_blocks; 522 VLOG(2) << "min row block size: " << options.min_row_block_size; 523 VLOG(2) << "max row block size: " << options.max_row_block_size; 524 VLOG(2) << "min col block size: " << options.min_col_block_size; 525 VLOG(2) << "max col block size: " << options.max_col_block_size; 526 VLOG(2) << "block density: " << options.block_density; 527 528 scoped_ptr<CompressedRowSparseMatrix> matrix( 529 CreateRandomCompressedRowSparseMatrix(options)); 530 531 cs_di cs_matrix_transpose = cxsparse.CreateSparseMatrixTransposeView(matrix.get()); 532 cs_di* cs_matrix = cxsparse.TransposeMatrix(&cs_matrix_transpose); 533 cs_di* expected_outer_product = 534 cxsparse.MatrixMatrixMultiply(&cs_matrix_transpose, cs_matrix); 535 536 vector<int> program; 537 scoped_ptr<CompressedRowSparseMatrix> outer_product( 538 CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram( 539 *matrix, &program)); 540 CompressedRowSparseMatrix::ComputeOuterProduct(*matrix, 541 program, 542 outer_product.get()); 543 544 cs_di actual_outer_product = 545 cxsparse.CreateSparseMatrixTransposeView(outer_product.get()); 546 547 ASSERT_EQ(actual_outer_product.m, actual_outer_product.n); 548 ASSERT_EQ(expected_outer_product->m, expected_outer_product->n); 549 ASSERT_EQ(actual_outer_product.m, expected_outer_product->m); 550 551 Matrix actual_matrix; 552 Matrix expected_matrix; 553 554 ToDenseMatrix(expected_outer_product, &expected_matrix); 555 expected_matrix.triangularView<Eigen::StrictlyLower>().setZero(); 556 557 ToDenseMatrix(&actual_outer_product, &actual_matrix); 558 const double diff_norm = (actual_matrix - expected_matrix).norm() / expected_matrix.norm(); 559 ASSERT_NEAR(diff_norm, 0.0, kTolerance) 560 << "expected: \n" 561 << expected_matrix 562 << "\nactual: \n" 563 << actual_matrix; 564 565 cxsparse.Free(cs_matrix); 566 cxsparse.Free(expected_outer_product); 567 } 568 } 569 } 570 } 571 572 #endif // CERES_NO_CXSPARSE 573 574 } // namespace internal 575 } // namespace ceres 576
