11d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Ceres Solver - A fast non-linear least squares minimizer 21d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Copyright 2013 Google Inc. All rights reserved. 31d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// http://code.google.com/p/ceres-solver/ 41d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 51d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Redistribution and use in source and binary forms, with or without 61d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// modification, are permitted provided that the following conditions are met: 71d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 81d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions of source code must retain the above copyright notice, 91d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer. 101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions in binary form must reproduce the above copyright notice, 111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer in the documentation 121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// and/or other materials provided with the distribution. 131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Neither the name of Google Inc. nor the names of its contributors may be 141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// used to endorse or promote products derived from this software without 151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// specific prior written permission. 161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// POSSIBILITY OF SUCH DAMAGE. 281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Author: sameeragarwal@google.com (Sameer Agarwal) 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <limits> 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <vector> 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/block_random_access_crs_matrix.h" 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/eigen.h" 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "gtest/gtest.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace ceres { 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace internal { 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST(BlockRandomAccessCRSMatrix, GetCell) { 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int> blocks; 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling blocks.push_back(3); 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling blocks.push_back(4); 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling blocks.push_back(5); 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_rows = 3 + 4 + 5; 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling set< pair<int, int> > block_pairs; 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int num_nonzeros = 0; 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling block_pairs.insert(make_pair(0, 0)); 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_nonzeros += blocks[0] * blocks[0]; 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling block_pairs.insert(make_pair(1, 1)); 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_nonzeros += blocks[1] * blocks[1]; 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling block_pairs.insert(make_pair(1, 2)); 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_nonzeros += blocks[1] * blocks[2]; 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling block_pairs.insert(make_pair(2, 0)); 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_nonzeros += blocks[2] * blocks[0]; 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BlockRandomAccessCRSMatrix m(blocks, block_pairs); 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(m.num_rows(), num_rows); 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(m.num_cols(), num_rows); 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling it != block_pairs.end(); 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++it) { 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int row_block_id = it->first; 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int col_block_id = it->second; 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int row; 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int col; 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int row_stride; 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int col_stride; 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CellInfo* cell = m.GetCell(row_block_id, col_block_id, 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &row, &col, 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &row_stride, &col_stride); 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cell != NULL); 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(row, 0); 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(col, 0); 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Write into the block. 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixRef(cell->values, row_stride, col_stride).block( 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling row, col, blocks[row_block_id], blocks[col_block_id]) = 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (row_block_id + 1) * (col_block_id +1) * 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Matrix::Ones(blocks[row_block_id], blocks[col_block_id]); 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const CompressedRowSparseMatrix* crsm = m.matrix(); 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(crsm->num_nonzeros(), num_nonzeros); 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Matrix dense; 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling crsm->ToDenseMatrix(&dense); 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double kTolerance = 1e-14; 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // (0,0) 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR((dense.block(0, 0, 3, 3) - Matrix::Ones(3, 3)).norm(), 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling kTolerance); 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // (1,1) 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR((dense.block(3, 3, 4, 4) - 2 * 2 * Matrix::Ones(4, 4)).norm(), 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling kTolerance); 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // (1,2) 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR((dense.block(3, 3 + 4, 4, 5) - 2 * 3 * Matrix::Ones(4, 5)).norm(), 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling kTolerance); 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // (2,0) 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR((dense.block(3 + 4, 0, 5, 3) - 3 * 1 * Matrix::Ones(5, 3)).norm(), 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0.0, 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling kTolerance); 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // There is nothing else in the matrix besides these four blocks. 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR(dense.norm(), sqrt(9. + 16. * 16. + 36. * 20. + 9. * 15.), 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling kTolerance); 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// IntPairToLong is private, thus this fixture is needed to access and 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// test it. 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass BlockRandomAccessCRSMatrixTest : public ::testing::Test { 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual void SetUp() { 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int> blocks; 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling blocks.push_back(1); 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling set< pair<int, int> > block_pairs; 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling block_pairs.insert(make_pair(0, 0)); 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m_.reset(new BlockRandomAccessCRSMatrix(blocks, block_pairs)); 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void CheckIntPair(int a, int b) { 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int64 value = m_->IntPairToLong(a, b); 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_GT(value, 0) << "Overflow a = " << a << " b = " << b; 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_GT(value, a) << "Overflow a = " << a << " b = " << b; 1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_GT(value, b) << "Overflow a = " << a << " b = " << b; 1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling private: 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_ptr<BlockRandomAccessCRSMatrix> m_; 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_F(BlockRandomAccessCRSMatrixTest, IntPairToLongOverflow) { 1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CheckIntPair(numeric_limits<int>::max(), numeric_limits<int>::max()); 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace internal 1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace ceres 149