10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 21d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Copyright 2010, 2011, 2012, 2013 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: keir@google.com (Keir Mierle) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// TODO(keir): Implement a generic "compare sparse matrix implementations" test 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// suite that can compare all the implementations. Then this file would shrink 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// in size. 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/dense_sparse_matrix.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/casts.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_least_squares_problems.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/triplet_sparse_matrix.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/eigen.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h" 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "gtest/gtest.h" 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CompareMatrices(const SparseMatrix* a, const SparseMatrix* b) { 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(a->num_rows(), b->num_rows()); 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(a->num_cols(), b->num_cols()); 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_rows = a->num_rows(); 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols = a->num_cols(); 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_cols; ++i) { 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector x = Vector::Zero(num_cols); 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x(i) = 1.0; 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector y_a = Vector::Zero(num_rows); 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector y_b = Vector::Zero(num_rows); 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong a->RightMultiply(x.data(), y_a.data()); 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong b->RightMultiply(x.data(), y_b.data()); 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ((y_a - y_b).norm(), 0); 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass DenseSparseMatrixTest : public ::testing::Test { 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong protected : 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void SetUp() { 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<LinearLeastSquaresProblem> problem( 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CreateLinearLeastSquaresProblemFromId(1)); 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(problem.get()); 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm.reset(down_cast<TripletSparseMatrix*>(problem->A.release())); 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dsm.reset(new DenseSparseMatrix(*tsm)); 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows = tsm->num_rows(); 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols = tsm->num_cols(); 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_rows; 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols; 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<TripletSparseMatrix> tsm; 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<DenseSparseMatrix> dsm; 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(DenseSparseMatrixTest, RightMultiply) { 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompareMatrices(tsm.get(), dsm.get()); 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Try with a not entirely zero vector to verify column interactions, which 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // could be masked by a subtle bug when using the elementary vectors. 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector a(num_cols); 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_cols; i++) { 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong a(i) = i; 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector b1 = Vector::Zero(num_rows); 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector b2 = Vector::Zero(num_rows); 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm->RightMultiply(a.data(), b1.data()); 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dsm->RightMultiply(a.data(), b2.data()); 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ((b1 - b2).norm(), 0); 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(DenseSparseMatrixTest, LeftMultiply) { 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_rows; ++i) { 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector a = Vector::Zero(num_rows); 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong a(i) = 1.0; 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector b1 = Vector::Zero(num_cols); 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector b2 = Vector::Zero(num_cols); 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm->LeftMultiply(a.data(), b1.data()); 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dsm->LeftMultiply(a.data(), b2.data()); 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ((b1 - b2).norm(), 0); 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Try with a not entirely zero vector to verify column interactions, which 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // could be masked by a subtle bug when using the elementary vectors. 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector a(num_rows); 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_rows; i++) { 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong a(i) = i; 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector b1 = Vector::Zero(num_cols); 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector b2 = Vector::Zero(num_cols); 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm->LeftMultiply(a.data(), b1.data()); 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dsm->LeftMultiply(a.data(), b2.data()); 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ((b1 - b2).norm(), 0); 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(DenseSparseMatrixTest, ColumnNorm) { 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector b1 = Vector::Zero(num_cols); 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector b2 = Vector::Zero(num_cols); 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm->SquaredColumnNorm(b1.data()); 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dsm->SquaredColumnNorm(b2.data()); 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ((b1 - b2).norm(), 0); 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(DenseSparseMatrixTest, Scale) { 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector scale(num_cols); 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_cols; ++i) { 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scale(i) = i + 1; 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm->ScaleColumns(scale.data()); 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dsm->ScaleColumns(scale.data()); 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompareMatrices(tsm.get(), dsm.get()); 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(DenseSparseMatrixTest, ToDenseMatrix) { 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix tsm_dense; 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix dsm_dense; 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tsm->ToDenseMatrix(&tsm_dense); 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dsm->ToDenseMatrix(&dsm_dense); 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ((tsm_dense - dsm_dense).norm(), 0.0); 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 170