10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 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: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/partitioned_matrix_view.h" 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_structure.h" 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/casts.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/eigen.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_least_squares_problems.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/random.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/sparse_matrix.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "gtest/gtest.h" 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongconst double kEpsilon = 1e-14; 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass PartitionedMatrixViewTest : public ::testing::Test { 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong protected : 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void SetUp() { 5279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez srand(5); 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<LinearLeastSquaresProblem> problem( 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CreateLinearLeastSquaresProblemFromId(2)); 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(problem.get()); 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_.reset(problem->A.release()); 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = A_->num_cols(); 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = A_->num_rows(); 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_eliminate_blocks_ = problem->num_eliminate_blocks; 6179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LinearSolver::Options options; 6279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez options.elimination_groups.push_back(num_eliminate_blocks_); 6379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez pmv_.reset(PartitionedMatrixViewBase::Create( 6479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez options, 6579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *down_cast<BlockSparseMatrix*>(A_.get()))); 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_rows_; 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols_; 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_eliminate_blocks_; 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<SparseMatrix> A_; 7279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez scoped_ptr<PartitionedMatrixViewBase> pmv_; 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(PartitionedMatrixViewTest, DimensionsTest) { 7679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(pmv_->num_col_blocks_e(), num_eliminate_blocks_); 7779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(pmv_->num_col_blocks_f(), num_cols_ - num_eliminate_blocks_); 7879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(pmv_->num_cols_e(), num_eliminate_blocks_); 7979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(pmv_->num_cols_f(), num_cols_ - num_eliminate_blocks_); 8079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(pmv_->num_cols(), A_->num_cols()); 8179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(pmv_->num_rows(), A_->num_rows()); 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(PartitionedMatrixViewTest, RightMultiplyE) { 8579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector x1(pmv_->num_cols_e()); 8679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector x2(pmv_->num_cols()); 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x2.setZero(); 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < pmv_->num_cols_e(); ++i) { 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x1(i) = x2(i) = RandDouble(); 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector y1 = Vector::Zero(pmv_->num_rows()); 9479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez pmv_->RightMultiplyE(x1.data(), y1.data()); 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector y2 = Vector::Zero(pmv_->num_rows()); 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->RightMultiply(x2.data(), y2.data()); 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 9979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < pmv_->num_rows(); ++i) { 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(y1(i), y2(i), kEpsilon); 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(PartitionedMatrixViewTest, RightMultiplyF) { 10579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector x1(pmv_->num_cols_f()); 10679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector x2 = Vector::Zero(pmv_->num_cols()); 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 10879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < pmv_->num_cols_f(); ++i) { 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x1(i) = RandDouble(); 11079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x2(i + pmv_->num_cols_e()) = x1(i); 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 11379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector y1 = Vector::Zero(pmv_->num_rows()); 11479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez pmv_->RightMultiplyF(x1.data(), y1.data()); 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 11679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector y2 = Vector::Zero(pmv_->num_rows()); 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->RightMultiply(x2.data(), y2.data()); 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 11979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < pmv_->num_rows(); ++i) { 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(y1(i), y2(i), kEpsilon); 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(PartitionedMatrixViewTest, LeftMultiply) { 12579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector x = Vector::Zero(pmv_->num_rows()); 12679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < pmv_->num_rows(); ++i) { 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x(i) = RandDouble(); 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 13079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector y = Vector::Zero(pmv_->num_cols()); 13179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector y1 = Vector::Zero(pmv_->num_cols_e()); 13279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector y2 = Vector::Zero(pmv_->num_cols_f()); 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->LeftMultiply(x.data(), y.data()); 13579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez pmv_->LeftMultiplyE(x.data(), y1.data()); 13679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez pmv_->LeftMultiplyF(x.data(), y2.data()); 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 13879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < pmv_->num_cols(); ++i) { 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(y(i), 14079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (i < pmv_->num_cols_e()) ? y1(i) : y2(i - pmv_->num_cols_e()), 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kEpsilon); 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(PartitionedMatrixViewTest, BlockDiagonalEtE) { 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<BlockSparseMatrix> 14779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez block_diagonal_ee(pmv_->CreateBlockDiagonalEtE()); 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs = block_diagonal_ee->block_structure(); 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(block_diagonal_ee->num_rows(), 2); 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(block_diagonal_ee->num_cols(), 2); 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(bs->cols.size(), 2); 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(bs->rows.size(), 2); 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(block_diagonal_ee->values()[0], 10.0, kEpsilon); 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(block_diagonal_ee->values()[1], 155.0, kEpsilon); 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(PartitionedMatrixViewTest, BlockDiagonalFtF) { 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<BlockSparseMatrix> 16179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez block_diagonal_ff(pmv_->CreateBlockDiagonalFtF()); 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs = block_diagonal_ff->block_structure(); 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(block_diagonal_ff->num_rows(), 3); 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(block_diagonal_ff->num_cols(), 3); 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(bs->cols.size(), 3); 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(bs->rows.size(), 3); 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(block_diagonal_ff->values()[0], 70.0, kEpsilon); 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(block_diagonal_ff->values()[1], 17.0, kEpsilon); 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(block_diagonal_ff->values()[2], 37.0, kEpsilon); 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 175