Home | History | Annotate | Download | only in ceres 
      1 // Ceres Solver - A fast non-linear least squares minimizer
      2 // Copyright 2013 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 <algorithm>
     32 #include "ceres/compressed_col_sparse_matrix_utils.h"
     33 #include "ceres/internal/port.h"
     34 #include "ceres/suitesparse.h"
     35 #include "ceres/triplet_sparse_matrix.h"
     36 #include "glog/logging.h"
     37 #include "gtest/gtest.h"
     38 
     39 namespace ceres {
     40 namespace internal {
     41 
     42 TEST(_, BlockPermutationToScalarPermutation) {
     43   vector<int> blocks;
     44   //  Block structure
     45   //  0  --1-  ---2---  ---3---  4
     46   // [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
     47   blocks.push_back(1);
     48   blocks.push_back(2);
     49   blocks.push_back(3);
     50   blocks.push_back(3);
     51   blocks.push_back(1);
     52 
     53   // Block ordering
     54   // [1, 0, 2, 4, 5]
     55   vector<int> block_ordering;
     56   block_ordering.push_back(1);
     57   block_ordering.push_back(0);
     58   block_ordering.push_back(2);
     59   block_ordering.push_back(4);
     60   block_ordering.push_back(3);
     61 
     62   // Expected ordering
     63   // [1, 2, 0, 3, 4, 5, 9, 6, 7, 8]
     64   vector<int> expected_scalar_ordering;
     65   expected_scalar_ordering.push_back(1);
     66   expected_scalar_ordering.push_back(2);
     67   expected_scalar_ordering.push_back(0);
     68   expected_scalar_ordering.push_back(3);
     69   expected_scalar_ordering.push_back(4);
     70   expected_scalar_ordering.push_back(5);
     71   expected_scalar_ordering.push_back(9);
     72   expected_scalar_ordering.push_back(6);
     73   expected_scalar_ordering.push_back(7);
     74   expected_scalar_ordering.push_back(8);
     75 
     76   vector<int> scalar_ordering;
     77   BlockOrderingToScalarOrdering(blocks,
     78                                 block_ordering,
     79                                 &scalar_ordering);
     80   EXPECT_EQ(scalar_ordering.size(), expected_scalar_ordering.size());
     81   for (int i = 0; i < expected_scalar_ordering.size(); ++i) {
     82     EXPECT_EQ(scalar_ordering[i], expected_scalar_ordering[i]);
     83   }
     84 }
     85 
     86 // Helper function to fill the sparsity pattern of a TripletSparseMatrix.
     87 int FillBlock(const vector<int>& row_blocks,
     88               const vector<int>& col_blocks,
     89               const int row_block_id,
     90               const int col_block_id,
     91               int* rows,
     92               int* cols) {
     93   int row_pos = 0;
     94   for (int i = 0; i < row_block_id; ++i) {
     95     row_pos += row_blocks[i];
     96   }
     97 
     98   int col_pos = 0;
     99   for (int i = 0; i < col_block_id; ++i) {
    100     col_pos += col_blocks[i];
    101   }
    102 
    103   int offset = 0;
    104   for (int r = 0; r < row_blocks[row_block_id]; ++r) {
    105     for (int c = 0; c < col_blocks[col_block_id]; ++c, ++offset) {
    106       rows[offset] = row_pos + r;
    107       cols[offset] = col_pos + c;
    108     }
    109   }
    110   return offset;
    111 }
    112 
    113 TEST(_, ScalarMatrixToBlockMatrix) {
    114   // Block sparsity.
    115   //
    116   //     [1 2 3 2]
    117   // [1]  x   x
    118   // [2]    x   x
    119   // [2]  x x
    120   // num_nonzeros = 1 + 3 + 4 + 4 + 1 + 2 = 15
    121 
    122   vector<int> col_blocks;
    123   col_blocks.push_back(1);
    124   col_blocks.push_back(2);
    125   col_blocks.push_back(3);
    126   col_blocks.push_back(2);
    127 
    128   vector<int> row_blocks;
    129   row_blocks.push_back(1);
    130   row_blocks.push_back(2);
    131   row_blocks.push_back(2);
    132 
    133   TripletSparseMatrix tsm(5, 8, 18);
    134   int* rows = tsm.mutable_rows();
    135   int* cols = tsm.mutable_cols();
    136   fill(tsm.mutable_values(), tsm.mutable_values() + 18, 1.0);
    137   int offset = 0;
    138 
    139 #define CERES_TEST_FILL_BLOCK(row_block_id, col_block_id) \
    140   offset += FillBlock(row_blocks, col_blocks, \
    141                       row_block_id, col_block_id, \
    142                       rows + offset, cols + offset);
    143 
    144   CERES_TEST_FILL_BLOCK(0, 0);
    145   CERES_TEST_FILL_BLOCK(2, 0);
    146   CERES_TEST_FILL_BLOCK(1, 1);
    147   CERES_TEST_FILL_BLOCK(2, 1);
    148   CERES_TEST_FILL_BLOCK(0, 2);
    149   CERES_TEST_FILL_BLOCK(1, 3);
    150 #undef CERES_TEST_FILL_BLOCK
    151 
    152   tsm.set_num_nonzeros(offset);
    153 
    154   SuiteSparse ss;
    155   scoped_ptr<cholmod_sparse> ccsm(ss.CreateSparseMatrix(&tsm));
    156 
    157   vector<int> expected_block_rows;
    158   expected_block_rows.push_back(0);
    159   expected_block_rows.push_back(2);
    160   expected_block_rows.push_back(1);
    161   expected_block_rows.push_back(2);
    162   expected_block_rows.push_back(0);
    163   expected_block_rows.push_back(1);
    164 
    165   vector<int> expected_block_cols;
    166   expected_block_cols.push_back(0);
    167   expected_block_cols.push_back(2);
    168   expected_block_cols.push_back(4);
    169   expected_block_cols.push_back(5);
    170   expected_block_cols.push_back(6);
    171 
    172   vector<int> block_rows;
    173   vector<int> block_cols;
    174   CompressedColumnScalarMatrixToBlockMatrix(
    175       reinterpret_cast<const int*>(ccsm->i),
    176       reinterpret_cast<const int*>(ccsm->p),
    177       row_blocks,
    178       col_blocks,
    179       &block_rows,
    180       &block_cols);
    181 
    182   EXPECT_EQ(block_cols.size(), expected_block_cols.size());
    183   EXPECT_EQ(block_rows.size(), expected_block_rows.size());
    184 
    185   for (int i = 0; i < expected_block_cols.size(); ++i) {
    186     EXPECT_EQ(block_cols[i], expected_block_cols[i]);
    187   }
    188 
    189   for (int i = 0; i < expected_block_rows.size(); ++i) {
    190     EXPECT_EQ(block_rows[i], expected_block_rows[i]);
    191   }
    192 
    193   ss.Free(ccsm.release());
    194 }
    195 
    196 class SolveUpperTriangularTest : public ::testing::Test {
    197  protected:
    198   void SetUp() {
    199     cols.resize(5);
    200     rows.resize(7);
    201     values.resize(7);
    202 
    203     cols[0] = 0;
    204     rows[0] = 0;
    205     values[0] = 0.50754;
    206 
    207     cols[1] = 1;
    208     rows[1] = 1;
    209     values[1] = 0.80483;
    210 
    211     cols[2] = 2;
    212     rows[2] = 1;
    213     values[2] = 0.14120;
    214     rows[3] = 2;
    215     values[3] = 0.3;
    216 
    217     cols[3] = 4;
    218     rows[4] = 0;
    219     values[4] = 0.77696;
    220     rows[5] = 1;
    221     values[5] = 0.41860;
    222     rows[6] = 3;
    223     values[6] = 0.88979;
    224 
    225     cols[4] = 7;
    226   }
    227 
    228   vector<int> cols;
    229   vector<int> rows;
    230   vector<double> values;
    231 };
    232 
    233 TEST_F(SolveUpperTriangularTest, SolveInPlace) {
    234   double rhs_and_solution[] = {1.0, 1.0, 2.0, 2.0};
    235   const double expected[] = { -1.4706, -1.0962, 6.6667, 2.2477};
    236 
    237   SolveUpperTriangularInPlace<int>(cols.size() - 1,
    238                                    &rows[0],
    239                                    &cols[0],
    240                                    &values[0],
    241                                    rhs_and_solution);
    242 
    243   for (int i = 0; i < 4; ++i) {
    244     EXPECT_NEAR(rhs_and_solution[i], expected[i], 1e-4) << i;
    245   }
    246 }
    247 
    248 TEST_F(SolveUpperTriangularTest, TransposeSolveInPlace) {
    249   double rhs_and_solution[] = {1.0, 1.0, 2.0, 2.0};
    250   double expected[] = {1.970288,  1.242498,  6.081864, -0.057255};
    251 
    252   SolveUpperTriangularTransposeInPlace<int>(cols.size() - 1,
    253                                             &rows[0],
    254                                             &cols[0],
    255                                             &values[0],
    256                                             rhs_and_solution);
    257 
    258   for (int i = 0; i < 4; ++i) {
    259     EXPECT_NEAR(rhs_and_solution[i], expected[i], 1e-4) << i;
    260   }
    261 }
    262 
    263 TEST_F(SolveUpperTriangularTest, RTRSolveWithSparseRHS) {
    264   double solution[4];
    265   double expected[] = { 6.8420e+00,   1.0057e+00,  -1.4907e-16,  -1.9335e+00,
    266                         1.0057e+00,   2.2275e+00,  -1.9493e+00,  -6.5693e-01,
    267                        -1.4907e-16,  -1.9493e+00,   1.1111e+01,   9.7381e-17,
    268                        -1.9335e+00,  -6.5693e-01,   9.7381e-17,   1.2631e+00 };
    269 
    270   for (int i = 0; i < 4; ++i) {
    271     SolveRTRWithSparseRHS<int>(cols.size() - 1,
    272                                &rows[0],
    273                                &cols[0],
    274                                &values[0],
    275                                i,
    276                                solution);
    277     for (int j = 0; j < 4; ++j) {
    278       EXPECT_NEAR(solution[j], expected[4 * i + j], 1e-3) << i;
    279     }
    280   }
    281 }
    282 
    283 }  // namespace internal
    284 }  // namespace ceres
    285