| 1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354 | // Ceres Solver - A fast non-linear least squares minimizer// Copyright 2015 Google Inc. All rights reserved.// http://ceres-solver.org///// Redistribution and use in source and binary forms, with or without// modification, are permitted provided that the following conditions are met://// * Redistributions of source code must retain the above copyright notice,//   this list of conditions and the following disclaimer.// * Redistributions in binary form must reproduce the above copyright notice,//   this list of conditions and the following disclaimer in the documentation//   and/or other materials provided with the distribution.// * Neither the name of Google Inc. nor the names of its contributors may be//   used to endorse or promote products derived from this software without//   specific prior written permission.//// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE// POSSIBILITY OF SUCH DAMAGE.//// Author: sameeragarwal@google.com (Sameer Agarwal)#include "ceres/covariance.h"#include <algorithm>#include <cmath>#include <cstdint>#include <map>#include <memory>#include <utility>#include "ceres/autodiff_cost_function.h"#include "ceres/compressed_row_sparse_matrix.h"#include "ceres/cost_function.h"#include "ceres/covariance_impl.h"#include "ceres/local_parameterization.h"#include "ceres/map_util.h"#include "ceres/problem_impl.h"#include "gtest/gtest.h"namespace ceres {namespace internal {using std::make_pair;using std::map;using std::pair;using std::vector;class UnaryCostFunction : public CostFunction { public:  UnaryCostFunction(const int num_residuals,                    const int32_t parameter_block_size,                    const double* jacobian)      : jacobian_(jacobian, jacobian + num_residuals * parameter_block_size) {    set_num_residuals(num_residuals);    mutable_parameter_block_sizes()->push_back(parameter_block_size);  }  bool Evaluate(double const* const* parameters,                double* residuals,                double** jacobians) const final {    for (int i = 0; i < num_residuals(); ++i) {      residuals[i] = 1;    }    if (jacobians == NULL) {      return true;    }    if (jacobians[0] != NULL) {      copy(jacobian_.begin(), jacobian_.end(), jacobians[0]);    }    return true;  } private:  vector<double> jacobian_;};class BinaryCostFunction : public CostFunction { public:  BinaryCostFunction(const int num_residuals,                     const int32_t parameter_block1_size,                     const int32_t parameter_block2_size,                     const double* jacobian1,                     const double* jacobian2)      : jacobian1_(jacobian1,                   jacobian1 + num_residuals * parameter_block1_size),        jacobian2_(jacobian2,                   jacobian2 + num_residuals * parameter_block2_size) {    set_num_residuals(num_residuals);    mutable_parameter_block_sizes()->push_back(parameter_block1_size);    mutable_parameter_block_sizes()->push_back(parameter_block2_size);  }  bool Evaluate(double const* const* parameters,                double* residuals,                double** jacobians) const final {    for (int i = 0; i < num_residuals(); ++i) {      residuals[i] = 2;    }    if (jacobians == NULL) {      return true;    }    if (jacobians[0] != NULL) {      copy(jacobian1_.begin(), jacobian1_.end(), jacobians[0]);    }    if (jacobians[1] != NULL) {      copy(jacobian2_.begin(), jacobian2_.end(), jacobians[1]);    }    return true;  } private:  vector<double> jacobian1_;  vector<double> jacobian2_;};// x_plus_delta = delta * x;class PolynomialParameterization : public LocalParameterization { public:  virtual ~PolynomialParameterization() {}  bool Plus(const double* x,            const double* delta,            double* x_plus_delta) const final {    x_plus_delta[0] = delta[0] * x[0];    x_plus_delta[1] = delta[0] * x[1];    return true;  }  bool ComputeJacobian(const double* x, double* jacobian) const final {    jacobian[0] = x[0];    jacobian[1] = x[1];    return true;  }  int GlobalSize() const final { return 2; }  int LocalSize() const final { return 1; }};TEST(CovarianceImpl, ComputeCovarianceSparsity) {  double parameters[10];  double* block1 = parameters;  double* block2 = block1 + 1;  double* block3 = block2 + 2;  double* block4 = block3 + 3;  ProblemImpl problem;  // Add in random order  Vector junk_jacobian = Vector::Zero(10);  problem.AddResidualBlock(      new UnaryCostFunction(1, 1, junk_jacobian.data()), NULL, block1);  problem.AddResidualBlock(      new UnaryCostFunction(1, 4, junk_jacobian.data()), NULL, block4);  problem.AddResidualBlock(      new UnaryCostFunction(1, 3, junk_jacobian.data()), NULL, block3);  problem.AddResidualBlock(      new UnaryCostFunction(1, 2, junk_jacobian.data()), NULL, block2);  // Sparsity pattern  //  // Note that the problem structure does not imply this sparsity  // pattern since all the residual blocks are unary. But the  // ComputeCovarianceSparsity function in its current incarnation  // does not pay attention to this fact and only looks at the  // parameter block pairs that the user provides.  //  //  X . . . . . X X X X  //  . X X X X X . . . .  //  . X X X X X . . . .  //  . . . X X X . . . .  //  . . . X X X . . . .  //  . . . X X X . . . .  //  . . . . . . X X X X  //  . . . . . . X X X X  //  . . . . . . X X X X  //  . . . . . . X X X X  // clang-format off  int expected_rows[] = {0, 5, 10, 15, 18, 21, 24, 28, 32, 36, 40};  int expected_cols[] = {0, 6, 7, 8, 9,                         1, 2, 3, 4, 5,                         1, 2, 3, 4, 5,                         3, 4, 5,                         3, 4, 5,                         3, 4, 5,                         6, 7, 8, 9,                         6, 7, 8, 9,                         6, 7, 8, 9,                         6, 7, 8, 9};  // clang-format on  vector<pair<const double*, const double*>> covariance_blocks;  covariance_blocks.push_back(make_pair(block1, block1));  covariance_blocks.push_back(make_pair(block4, block4));  covariance_blocks.push_back(make_pair(block2, block2));  covariance_blocks.push_back(make_pair(block3, block3));  covariance_blocks.push_back(make_pair(block2, block3));  covariance_blocks.push_back(make_pair(block4, block1));  // reversed  Covariance::Options options;  CovarianceImpl covariance_impl(options);  EXPECT_TRUE(      covariance_impl.ComputeCovarianceSparsity(covariance_blocks, &problem));  const CompressedRowSparseMatrix* crsm = covariance_impl.covariance_matrix();  EXPECT_EQ(crsm->num_rows(), 10);  EXPECT_EQ(crsm->num_cols(), 10);  EXPECT_EQ(crsm->num_nonzeros(), 40);  const int* rows = crsm->rows();  for (int r = 0; r < crsm->num_rows() + 1; ++r) {    EXPECT_EQ(rows[r], expected_rows[r])        << r << " " << rows[r] << " " << expected_rows[r];  }  const int* cols = crsm->cols();  for (int c = 0; c < crsm->num_nonzeros(); ++c) {    EXPECT_EQ(cols[c], expected_cols[c])        << c << " " << cols[c] << " " << expected_cols[c];  }}TEST(CovarianceImpl, ComputeCovarianceSparsityWithConstantParameterBlock) {  double parameters[10];  double* block1 = parameters;  double* block2 = block1 + 1;  double* block3 = block2 + 2;  double* block4 = block3 + 3;  ProblemImpl problem;  // Add in random order  Vector junk_jacobian = Vector::Zero(10);  problem.AddResidualBlock(      new UnaryCostFunction(1, 1, junk_jacobian.data()), NULL, block1);  problem.AddResidualBlock(      new UnaryCostFunction(1, 4, junk_jacobian.data()), NULL, block4);  problem.AddResidualBlock(      new UnaryCostFunction(1, 3, junk_jacobian.data()), NULL, block3);  problem.AddResidualBlock(      new UnaryCostFunction(1, 2, junk_jacobian.data()), NULL, block2);  problem.SetParameterBlockConstant(block3);  // Sparsity pattern  //  // Note that the problem structure does not imply this sparsity  // pattern since all the residual blocks are unary. But the  // ComputeCovarianceSparsity function in its current incarnation  // does not pay attention to this fact and only looks at the  // parameter block pairs that the user provides.  //  //  X . . X X X X  //  . X X . . . .  //  . X X . . . .  //  . . . X X X X  //  . . . X X X X  //  . . . X X X X  //  . . . X X X X  // clang-format off  int expected_rows[] = {0, 5, 7, 9, 13, 17, 21, 25};  int expected_cols[] = {0, 3, 4, 5, 6,                         1, 2,                         1, 2,                         3, 4, 5, 6,                         3, 4, 5, 6,                         3, 4, 5, 6,                         3, 4, 5, 6};  // clang-format on  vector<pair<const double*, const double*>> covariance_blocks;  covariance_blocks.push_back(make_pair(block1, block1));  covariance_blocks.push_back(make_pair(block4, block4));  covariance_blocks.push_back(make_pair(block2, block2));  covariance_blocks.push_back(make_pair(block3, block3));  covariance_blocks.push_back(make_pair(block2, block3));  covariance_blocks.push_back(make_pair(block4, block1));  // reversed  Covariance::Options options;  CovarianceImpl covariance_impl(options);  EXPECT_TRUE(      covariance_impl.ComputeCovarianceSparsity(covariance_blocks, &problem));  const CompressedRowSparseMatrix* crsm = covariance_impl.covariance_matrix();  EXPECT_EQ(crsm->num_rows(), 7);  EXPECT_EQ(crsm->num_cols(), 7);  EXPECT_EQ(crsm->num_nonzeros(), 25);  const int* rows = crsm->rows();  for (int r = 0; r < crsm->num_rows() + 1; ++r) {    EXPECT_EQ(rows[r], expected_rows[r])        << r << " " << rows[r] << " " << expected_rows[r];  }  const int* cols = crsm->cols();  for (int c = 0; c < crsm->num_nonzeros(); ++c) {    EXPECT_EQ(cols[c], expected_cols[c])        << c << " " << cols[c] << " " << expected_cols[c];  }}TEST(CovarianceImpl, ComputeCovarianceSparsityWithFreeParameterBlock) {  double parameters[10];  double* block1 = parameters;  double* block2 = block1 + 1;  double* block3 = block2 + 2;  double* block4 = block3 + 3;  ProblemImpl problem;  // Add in random order  Vector junk_jacobian = Vector::Zero(10);  problem.AddResidualBlock(      new UnaryCostFunction(1, 1, junk_jacobian.data()), NULL, block1);  problem.AddResidualBlock(      new UnaryCostFunction(1, 4, junk_jacobian.data()), NULL, block4);  problem.AddParameterBlock(block3, 3);  problem.AddResidualBlock(      new UnaryCostFunction(1, 2, junk_jacobian.data()), NULL, block2);  // Sparsity pattern  //  // Note that the problem structure does not imply this sparsity  // pattern since all the residual blocks are unary. But the  // ComputeCovarianceSparsity function in its current incarnation  // does not pay attention to this fact and only looks at the  // parameter block pairs that the user provides.  //  //  X . . X X X X  //  . X X . . . .  //  . X X . . . .  //  . . . X X X X  //  . . . X X X X  //  . . . X X X X  //  . . . X X X X  // clang-format off  int expected_rows[] = {0, 5, 7, 9, 13, 17, 21, 25};  int expected_cols[] = {0, 3, 4, 5, 6,                         1, 2,                         1, 2,                         3, 4, 5, 6,                         3, 4, 5, 6,                         3, 4, 5, 6,                         3, 4, 5, 6};  // clang-format on  vector<pair<const double*, const double*>> covariance_blocks;  covariance_blocks.push_back(make_pair(block1, block1));  covariance_blocks.push_back(make_pair(block4, block4));  covariance_blocks.push_back(make_pair(block2, block2));  covariance_blocks.push_back(make_pair(block3, block3));  covariance_blocks.push_back(make_pair(block2, block3));  covariance_blocks.push_back(make_pair(block4, block1));  // reversed  Covariance::Options options;  CovarianceImpl covariance_impl(options);  EXPECT_TRUE(      covariance_impl.ComputeCovarianceSparsity(covariance_blocks, &problem));  const CompressedRowSparseMatrix* crsm = covariance_impl.covariance_matrix();  EXPECT_EQ(crsm->num_rows(), 7);  EXPECT_EQ(crsm->num_cols(), 7);  EXPECT_EQ(crsm->num_nonzeros(), 25);  const int* rows = crsm->rows();  for (int r = 0; r < crsm->num_rows() + 1; ++r) {    EXPECT_EQ(rows[r], expected_rows[r])        << r << " " << rows[r] << " " << expected_rows[r];  }  const int* cols = crsm->cols();  for (int c = 0; c < crsm->num_nonzeros(); ++c) {    EXPECT_EQ(cols[c], expected_cols[c])        << c << " " << cols[c] << " " << expected_cols[c];  }}class CovarianceTest : public ::testing::Test { protected:  typedef map<const double*, pair<int, int>> BoundsMap;  void SetUp() override {    double* x = parameters_;    double* y = x + 2;    double* z = y + 3;    x[0] = 1;    x[1] = 1;    y[0] = 2;    y[1] = 2;    y[2] = 2;    z[0] = 3;    {      double jacobian[] = {1.0, 0.0, 0.0, 1.0};      problem_.AddResidualBlock(new UnaryCostFunction(2, 2, jacobian), NULL, x);    }    {      double jacobian[] = {2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0};      problem_.AddResidualBlock(new UnaryCostFunction(3, 3, jacobian), NULL, y);    }    {      double jacobian = 5.0;      problem_.AddResidualBlock(          new UnaryCostFunction(1, 1, &jacobian), NULL, z);    }    {      double jacobian1[] = {1.0, 2.0, 3.0};      double jacobian2[] = {-5.0, -6.0};      problem_.AddResidualBlock(          new BinaryCostFunction(1, 3, 2, jacobian1, jacobian2), NULL, y, x);    }    {      double jacobian1[] = {2.0};      double jacobian2[] = {3.0, -2.0};      problem_.AddResidualBlock(          new BinaryCostFunction(1, 1, 2, jacobian1, jacobian2), NULL, z, x);    }    all_covariance_blocks_.push_back(make_pair(x, x));    all_covariance_blocks_.push_back(make_pair(y, y));    all_covariance_blocks_.push_back(make_pair(z, z));    all_covariance_blocks_.push_back(make_pair(x, y));    all_covariance_blocks_.push_back(make_pair(x, z));    all_covariance_blocks_.push_back(make_pair(y, z));    column_bounds_[x] = make_pair(0, 2);    column_bounds_[y] = make_pair(2, 5);    column_bounds_[z] = make_pair(5, 6);  }  // Computes covariance in ambient space.  void ComputeAndCompareCovarianceBlocks(const Covariance::Options& options,                                         const double* expected_covariance) {    ComputeAndCompareCovarianceBlocksInTangentOrAmbientSpace(        options,        true,  // ambient        expected_covariance);  }  // Computes covariance in tangent space.  void ComputeAndCompareCovarianceBlocksInTangentSpace(      const Covariance::Options& options, const double* expected_covariance) {    ComputeAndCompareCovarianceBlocksInTangentOrAmbientSpace(        options,        false,  // tangent        expected_covariance);  }  void ComputeAndCompareCovarianceBlocksInTangentOrAmbientSpace(      const Covariance::Options& options,      bool lift_covariance_to_ambient_space,      const double* expected_covariance) {    // Generate all possible combination of block pairs and check if the    // covariance computation is correct.    for (int i = 0; i <= 64; ++i) {      vector<pair<const double*, const double*>> covariance_blocks;      if (i & 1) {        covariance_blocks.push_back(all_covariance_blocks_[0]);      }      if (i & 2) {        covariance_blocks.push_back(all_covariance_blocks_[1]);      }      if (i & 4) {        covariance_blocks.push_back(all_covariance_blocks_[2]);      }      if (i & 8) {        covariance_blocks.push_back(all_covariance_blocks_[3]);      }      if (i & 16) {        covariance_blocks.push_back(all_covariance_blocks_[4]);      }      if (i & 32) {        covariance_blocks.push_back(all_covariance_blocks_[5]);      }      Covariance covariance(options);      EXPECT_TRUE(covariance.Compute(covariance_blocks, &problem_));      for (int i = 0; i < covariance_blocks.size(); ++i) {        const double* block1 = covariance_blocks[i].first;        const double* block2 = covariance_blocks[i].second;        // block1, block2        GetCovarianceBlockAndCompare(block1,                                     block2,                                     lift_covariance_to_ambient_space,                                     covariance,                                     expected_covariance);        // block2, block1        GetCovarianceBlockAndCompare(block2,                                     block1,                                     lift_covariance_to_ambient_space,                                     covariance,                                     expected_covariance);      }    }  }  void GetCovarianceBlockAndCompare(const double* block1,                                    const double* block2,                                    bool lift_covariance_to_ambient_space,                                    const Covariance& covariance,                                    const double* expected_covariance) {    const BoundsMap& column_bounds = lift_covariance_to_ambient_space                                         ? column_bounds_                                         : local_column_bounds_;    const int row_begin = FindOrDie(column_bounds, block1).first;    const int row_end = FindOrDie(column_bounds, block1).second;    const int col_begin = FindOrDie(column_bounds, block2).first;    const int col_end = FindOrDie(column_bounds, block2).second;    Matrix actual(row_end - row_begin, col_end - col_begin);    if (lift_covariance_to_ambient_space) {      EXPECT_TRUE(covariance.GetCovarianceBlock(block1, block2, actual.data()));    } else {      EXPECT_TRUE(covariance.GetCovarianceBlockInTangentSpace(          block1, block2, actual.data()));    }    int dof = 0;  // degrees of freedom = sum of LocalSize()s    for (const auto& bound : column_bounds) {      dof = std::max(dof, bound.second.second);    }    ConstMatrixRef expected(expected_covariance, dof, dof);    double diff_norm =        (expected.block(             row_begin, col_begin, row_end - row_begin, col_end - col_begin) -         actual)            .norm();    diff_norm /= (row_end - row_begin) * (col_end - col_begin);    const double kTolerance = 1e-5;    EXPECT_NEAR(diff_norm, 0.0, kTolerance)        << "rows: " << row_begin << " " << row_end << "  "        << "cols: " << col_begin << " " << col_end << "  "        << "\n\n expected: \n "        << expected.block(               row_begin, col_begin, row_end - row_begin, col_end - col_begin)        << "\n\n actual: \n " << actual << "\n\n full expected: \n"        << expected;  }  double parameters_[6];  Problem problem_;  vector<pair<const double*, const double*>> all_covariance_blocks_;  BoundsMap column_bounds_;  BoundsMap local_column_bounds_;};TEST_F(CovarianceTest, NormalBehavior) {  // J  //  //   1  0  0  0  0  0  //   0  1  0  0  0  0  //   0  0  2  0  0  0  //   0  0  0  2  0  0  //   0  0  0  0  2  0  //   0  0  0  0  0  5  //  -5 -6  1  2  3  0  //   3 -2  0  0  0  2  // J'J  //  //   35  24 -5 -10 -15  6  //   24  41 -6 -12 -18 -4  //   -5  -6  5   2   3  0  //  -10 -12  2   8   6  0  //  -15 -18  3   6  13  0  //    6  -4  0   0   0 29  // inv(J'J) computed using octave.  // clang-format off  double expected_covariance[] = {     7.0747e-02,  -8.4923e-03,   1.6821e-02,   3.3643e-02,   5.0464e-02,  -1.5809e-02,  // NOLINT    -8.4923e-03,   8.1352e-02,   2.4758e-02,   4.9517e-02,   7.4275e-02,   1.2978e-02,  // NOLINT     1.6821e-02,   2.4758e-02,   2.4904e-01,  -1.9271e-03,  -2.8906e-03,  -6.5325e-05,  // NOLINT     3.3643e-02,   4.9517e-02,  -1.9271e-03,   2.4615e-01,  -5.7813e-03,  -1.3065e-04,  // NOLINT     5.0464e-02,   7.4275e-02,  -2.8906e-03,  -5.7813e-03,   2.4133e-01,  -1.9598e-04,  // NOLINT    -1.5809e-02,   1.2978e-02,  -6.5325e-05,  -1.3065e-04,  -1.9598e-04,   3.9544e-02,  // NOLINT  };  // clang-format on  Covariance::Options options;#ifndef CERES_NO_SUITESPARSE  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = SUITE_SPARSE;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);#endif  options.algorithm_type = DENSE_SVD;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = EIGEN_SPARSE;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);}#ifdef CERES_USE_OPENMPTEST_F(CovarianceTest, ThreadedNormalBehavior) {  // J  //  //   1  0  0  0  0  0  //   0  1  0  0  0  0  //   0  0  2  0  0  0  //   0  0  0  2  0  0  //   0  0  0  0  2  0  //   0  0  0  0  0  5  //  -5 -6  1  2  3  0  //   3 -2  0  0  0  2  // J'J  //  //   35  24 -5 -10 -15  6  //   24  41 -6 -12 -18 -4  //   -5  -6  5   2   3  0  //  -10 -12  2   8   6  0  //  -15 -18  3   6  13  0  //    6  -4  0   0   0 29  // inv(J'J) computed using octave.  // clang-format off  double expected_covariance[] = {     7.0747e-02,  -8.4923e-03,   1.6821e-02,   3.3643e-02,   5.0464e-02,  -1.5809e-02,  // NOLINT    -8.4923e-03,   8.1352e-02,   2.4758e-02,   4.9517e-02,   7.4275e-02,   1.2978e-02,  // NOLINT     1.6821e-02,   2.4758e-02,   2.4904e-01,  -1.9271e-03,  -2.8906e-03,  -6.5325e-05,  // NOLINT     3.3643e-02,   4.9517e-02,  -1.9271e-03,   2.4615e-01,  -5.7813e-03,  -1.3065e-04,  // NOLINT     5.0464e-02,   7.4275e-02,  -2.8906e-03,  -5.7813e-03,   2.4133e-01,  -1.9598e-04,  // NOLINT    -1.5809e-02,   1.2978e-02,  -6.5325e-05,  -1.3065e-04,  -1.9598e-04,   3.9544e-02,  // NOLINT  };  // clang-format on  Covariance::Options options;  options.num_threads = 4;#ifndef CERES_NO_SUITESPARSE  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = SUITE_SPARSE;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);#endif  options.algorithm_type = DENSE_SVD;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = EIGEN_SPARSE;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);}#endif  // CERES_USE_OPENMPTEST_F(CovarianceTest, ConstantParameterBlock) {  problem_.SetParameterBlockConstant(parameters_);  // J  //  //  0  0  0  0  0  0  //  0  0  0  0  0  0  //  0  0  2  0  0  0  //  0  0  0  2  0  0  //  0  0  0  0  2  0  //  0  0  0  0  0  5  //  0  0  1  2  3  0  //  0  0  0  0  0  2  // J'J  //  //  0  0  0  0  0  0  //  0  0  0  0  0  0  //  0  0  5  2  3  0  //  0  0  2  8  6  0  //  0  0  3  6 13  0  //  0  0  0  0  0 29  // pinv(J'J) computed using octave.  // clang-format off  double expected_covariance[] = {              0,            0,            0,            0,            0,            0,  // NOLINT              0,            0,            0,            0,            0,            0,  // NOLINT              0,            0,      0.23611,     -0.02778,     -0.04167,     -0.00000,  // NOLINT              0,            0,     -0.02778,      0.19444,     -0.08333,     -0.00000,  // NOLINT              0,            0,     -0.04167,     -0.08333,      0.12500,     -0.00000,  // NOLINT              0,            0,     -0.00000,     -0.00000,     -0.00000,      0.03448   // NOLINT      // clang-format on  };  Covariance::Options options;#ifndef CERES_NO_SUITESPARSE  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = SUITE_SPARSE;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);#endif  options.algorithm_type = DENSE_SVD;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = EIGEN_SPARSE;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);}TEST_F(CovarianceTest, LocalParameterization) {  double* x = parameters_;  double* y = x + 2;  problem_.SetParameterization(x, new PolynomialParameterization);  vector<int> subset;  subset.push_back(2);  problem_.SetParameterization(y, new SubsetParameterization(3, subset));  // Raw Jacobian: J  //  //   1   0  0  0  0  0  //   0   1  0  0  0  0  //   0   0  2  0  0  0  //   0   0  0  2  0  0  //   0   0  0  0  2  0  //   0   0  0  0  0  5  //  -5  -6  1  2  3  0  //   3  -2  0  0  0  2  // Local to global jacobian: A  //  //  1   0   0   0  //  1   0   0   0  //  0   1   0   0  //  0   0   1   0  //  0   0   0   0  //  0   0   0   1  // A * inv((J*A)'*(J*A)) * A'  // Computed using octave.  // clang-format off  double expected_covariance[] = {    0.01766,   0.01766,   0.02158,   0.04316,   0.00000,  -0.00122,    0.01766,   0.01766,   0.02158,   0.04316,   0.00000,  -0.00122,    0.02158,   0.02158,   0.24860,  -0.00281,   0.00000,  -0.00149,    0.04316,   0.04316,  -0.00281,   0.24439,   0.00000,  -0.00298,    0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   -0.00122,  -0.00122,  -0.00149,  -0.00298,   0.00000,   0.03457  };  // clang-format on  Covariance::Options options;#ifndef CERES_NO_SUITESPARSE  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = SUITE_SPARSE;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);#endif  options.algorithm_type = DENSE_SVD;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = EIGEN_SPARSE;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);}TEST_F(CovarianceTest, LocalParameterizationInTangentSpace) {  double* x = parameters_;  double* y = x + 2;  double* z = y + 3;  problem_.SetParameterization(x, new PolynomialParameterization);  vector<int> subset;  subset.push_back(2);  problem_.SetParameterization(y, new SubsetParameterization(3, subset));  local_column_bounds_[x] = make_pair(0, 1);  local_column_bounds_[y] = make_pair(1, 3);  local_column_bounds_[z] = make_pair(3, 4);  // Raw Jacobian: J  //  //   1   0  0  0  0  0  //   0   1  0  0  0  0  //   0   0  2  0  0  0  //   0   0  0  2  0  0  //   0   0  0  0  2  0  //   0   0  0  0  0  5  //  -5  -6  1  2  3  0  //   3  -2  0  0  0  2  // Local to global jacobian: A  //  //  1   0   0   0  //  1   0   0   0  //  0   1   0   0  //  0   0   1   0  //  0   0   0   0  //  0   0   0   1  // inv((J*A)'*(J*A))  // Computed using octave.  // clang-format off  double expected_covariance[] = {    0.01766,   0.02158,   0.04316,   -0.00122,    0.02158,   0.24860,  -0.00281,   -0.00149,    0.04316,  -0.00281,   0.24439,   -0.00298,   -0.00122,  -0.00149,  -0.00298,    0.03457  // NOLINT  };  // clang-format on  Covariance::Options options;#ifndef CERES_NO_SUITESPARSE  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = SUITE_SPARSE;  ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);#endif  options.algorithm_type = DENSE_SVD;  ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = EIGEN_SPARSE;  ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);}TEST_F(CovarianceTest, LocalParameterizationInTangentSpaceWithConstantBlocks) {  double* x = parameters_;  double* y = x + 2;  double* z = y + 3;  problem_.SetParameterization(x, new PolynomialParameterization);  problem_.SetParameterBlockConstant(x);  vector<int> subset;  subset.push_back(2);  problem_.SetParameterization(y, new SubsetParameterization(3, subset));  problem_.SetParameterBlockConstant(y);  local_column_bounds_[x] = make_pair(0, 1);  local_column_bounds_[y] = make_pair(1, 3);  local_column_bounds_[z] = make_pair(3, 4);  // Raw Jacobian: J  //  //   1   0  0  0  0  0  //   0   1  0  0  0  0  //   0   0  2  0  0  0  //   0   0  0  2  0  0  //   0   0  0  0  2  0  //   0   0  0  0  0  5  //  -5  -6  1  2  3  0  //   3  -2  0  0  0  2  // Local to global jacobian: A  //  //  0   0   0   0  //  0   0   0   0  //  0   0   0   0  //  0   0   0   0  //  0   0   0   0  //  0   0   0   1  // pinv((J*A)'*(J*A))  // Computed using octave.  // clang-format off  double expected_covariance[] = {    0.0, 0.0, 0.0, 0.0,    0.0, 0.0, 0.0, 0.0,    0.0, 0.0, 0.0, 0.0,    0.0, 0.0, 0.0, 0.034482 // NOLINT  };  // clang-format on  Covariance::Options options;#ifndef CERES_NO_SUITESPARSE  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = SUITE_SPARSE;  ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);#endif  options.algorithm_type = DENSE_SVD;  ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = EIGEN_SPARSE;  ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);}TEST_F(CovarianceTest, TruncatedRank) {  // J  //  //   1  0  0  0  0  0  //   0  1  0  0  0  0  //   0  0  2  0  0  0  //   0  0  0  2  0  0  //   0  0  0  0  2  0  //   0  0  0  0  0  5  //  -5 -6  1  2  3  0  //   3 -2  0  0  0  2  // J'J  //  //   35  24 -5 -10 -15  6  //   24  41 -6 -12 -18 -4  //   -5  -6  5   2   3  0  //  -10 -12  2   8   6  0  //  -15 -18  3   6  13  0  //    6  -4  0   0   0 29  // 3.4142 is the smallest eigen value of J'J. The following matrix  // was obtained by dropping the eigenvector corresponding to this  // eigenvalue.  // clang-format off  double expected_covariance[] = {     5.4135e-02,  -3.5121e-02,   1.7257e-04,   3.4514e-04,   5.1771e-04,  -1.6076e-02,  // NOLINT    -3.5121e-02,   3.8667e-02,  -1.9288e-03,  -3.8576e-03,  -5.7864e-03,   1.2549e-02,  // NOLINT     1.7257e-04,  -1.9288e-03,   2.3235e-01,  -3.5297e-02,  -5.2946e-02,  -3.3329e-04,  // NOLINT     3.4514e-04,  -3.8576e-03,  -3.5297e-02,   1.7941e-01,  -1.0589e-01,  -6.6659e-04,  // NOLINT     5.1771e-04,  -5.7864e-03,  -5.2946e-02,  -1.0589e-01,   9.1162e-02,  -9.9988e-04,  // NOLINT    -1.6076e-02,   1.2549e-02,  -3.3329e-04,  -6.6659e-04,  -9.9988e-04,   3.9539e-02   // NOLINT  };  // clang-format on  {    Covariance::Options options;    options.algorithm_type = DENSE_SVD;    // Force dropping of the smallest eigenvector.    options.null_space_rank = 1;    ComputeAndCompareCovarianceBlocks(options, expected_covariance);  }  {    Covariance::Options options;    options.algorithm_type = DENSE_SVD;    // Force dropping of the smallest eigenvector via the ratio but    // automatic truncation.    options.min_reciprocal_condition_number = 0.044494;    options.null_space_rank = -1;    ComputeAndCompareCovarianceBlocks(options, expected_covariance);  }}TEST_F(CovarianceTest, DenseCovarianceMatrixFromSetOfParameters) {  Covariance::Options options;  Covariance covariance(options);  double* x = parameters_;  double* y = x + 2;  double* z = y + 3;  vector<const double*> parameter_blocks;  parameter_blocks.push_back(x);  parameter_blocks.push_back(y);  parameter_blocks.push_back(z);  covariance.Compute(parameter_blocks, &problem_);  double expected_covariance[36];  covariance.GetCovarianceMatrix(parameter_blocks, expected_covariance);#ifndef CERES_NO_SUITESPARSE  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = SUITE_SPARSE;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);#endif  options.algorithm_type = DENSE_SVD;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = EIGEN_SPARSE;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);}TEST_F(CovarianceTest, DenseCovarianceMatrixFromSetOfParametersThreaded) {  Covariance::Options options;  options.num_threads = 4;  Covariance covariance(options);  double* x = parameters_;  double* y = x + 2;  double* z = y + 3;  vector<const double*> parameter_blocks;  parameter_blocks.push_back(x);  parameter_blocks.push_back(y);  parameter_blocks.push_back(z);  covariance.Compute(parameter_blocks, &problem_);  double expected_covariance[36];  covariance.GetCovarianceMatrix(parameter_blocks, expected_covariance);#ifndef CERES_NO_SUITESPARSE  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = SUITE_SPARSE;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);#endif  options.algorithm_type = DENSE_SVD;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = EIGEN_SPARSE;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);}TEST_F(CovarianceTest, DenseCovarianceMatrixFromSetOfParametersInTangentSpace) {  Covariance::Options options;  Covariance covariance(options);  double* x = parameters_;  double* y = x + 2;  double* z = y + 3;  problem_.SetParameterization(x, new PolynomialParameterization);  vector<int> subset;  subset.push_back(2);  problem_.SetParameterization(y, new SubsetParameterization(3, subset));  local_column_bounds_[x] = make_pair(0, 1);  local_column_bounds_[y] = make_pair(1, 3);  local_column_bounds_[z] = make_pair(3, 4);  vector<const double*> parameter_blocks;  parameter_blocks.push_back(x);  parameter_blocks.push_back(y);  parameter_blocks.push_back(z);  covariance.Compute(parameter_blocks, &problem_);  double expected_covariance[16];  covariance.GetCovarianceMatrixInTangentSpace(parameter_blocks,                                               expected_covariance);#ifndef CERES_NO_SUITESPARSE  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = SUITE_SPARSE;  ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);#endif  options.algorithm_type = DENSE_SVD;  ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);  options.algorithm_type = SPARSE_QR;  options.sparse_linear_algebra_library_type = EIGEN_SPARSE;  ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance);}TEST_F(CovarianceTest, ComputeCovarianceFailure) {  Covariance::Options options;  Covariance covariance(options);  double* x = parameters_;  double* y = x + 2;  vector<const double*> parameter_blocks;  parameter_blocks.push_back(x);  parameter_blocks.push_back(x);  parameter_blocks.push_back(y);  parameter_blocks.push_back(y);  EXPECT_DEATH_IF_SUPPORTED(covariance.Compute(parameter_blocks, &problem_),                            "Covariance::Compute called with duplicate blocks "                            "at indices \\(0, 1\\) and \\(2, 3\\)");  vector<pair<const double*, const double*>> covariance_blocks;  covariance_blocks.push_back(make_pair(x, x));  covariance_blocks.push_back(make_pair(x, x));  covariance_blocks.push_back(make_pair(y, y));  covariance_blocks.push_back(make_pair(y, y));  EXPECT_DEATH_IF_SUPPORTED(covariance.Compute(covariance_blocks, &problem_),                            "Covariance::Compute called with duplicate blocks "                            "at indices \\(0, 1\\) and \\(2, 3\\)");}class RankDeficientCovarianceTest : public CovarianceTest { protected:  void SetUp() final {    double* x = parameters_;    double* y = x + 2;    double* z = y + 3;    {      double jacobian[] = {1.0, 0.0, 0.0, 1.0};      problem_.AddResidualBlock(new UnaryCostFunction(2, 2, jacobian), NULL, x);    }    {      double jacobian[] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};      problem_.AddResidualBlock(new UnaryCostFunction(3, 3, jacobian), NULL, y);    }    {      double jacobian = 5.0;      problem_.AddResidualBlock(          new UnaryCostFunction(1, 1, &jacobian), NULL, z);    }    {      double jacobian1[] = {0.0, 0.0, 0.0};      double jacobian2[] = {-5.0, -6.0};      problem_.AddResidualBlock(          new BinaryCostFunction(1, 3, 2, jacobian1, jacobian2), NULL, y, x);    }    {      double jacobian1[] = {2.0};      double jacobian2[] = {3.0, -2.0};      problem_.AddResidualBlock(          new BinaryCostFunction(1, 1, 2, jacobian1, jacobian2), NULL, z, x);    }    all_covariance_blocks_.push_back(make_pair(x, x));    all_covariance_blocks_.push_back(make_pair(y, y));    all_covariance_blocks_.push_back(make_pair(z, z));    all_covariance_blocks_.push_back(make_pair(x, y));    all_covariance_blocks_.push_back(make_pair(x, z));    all_covariance_blocks_.push_back(make_pair(y, z));    column_bounds_[x] = make_pair(0, 2);    column_bounds_[y] = make_pair(2, 5);    column_bounds_[z] = make_pair(5, 6);  }};TEST_F(RankDeficientCovarianceTest, AutomaticTruncation) {  // J  //  //   1  0  0  0  0  0  //   0  1  0  0  0  0  //   0  0  0  0  0  0  //   0  0  0  0  0  0  //   0  0  0  0  0  0  //   0  0  0  0  0  5  //  -5 -6  0  0  0  0  //   3 -2  0  0  0  2  // J'J  //  //  35 24  0  0  0  6  //  24 41  0  0  0 -4  //   0  0  0  0  0  0  //   0  0  0  0  0  0  //   0  0  0  0  0  0  //   6 -4  0  0  0 29  // pinv(J'J) computed using octave.  // clang-format off  double expected_covariance[] = {     0.053998,  -0.033145,   0.000000,   0.000000,   0.000000,  -0.015744,    -0.033145,   0.045067,   0.000000,   0.000000,   0.000000,   0.013074,     0.000000,   0.000000,   0.000000,   0.000000,   0.000000,   0.000000,     0.000000,   0.000000,   0.000000,   0.000000,   0.000000,   0.000000,     0.000000,   0.000000,   0.000000,   0.000000,   0.000000,   0.000000,    -0.015744,   0.013074,   0.000000,   0.000000,   0.000000,   0.039543  };  // clang-format on  Covariance::Options options;  options.algorithm_type = DENSE_SVD;  options.null_space_rank = -1;  ComputeAndCompareCovarianceBlocks(options, expected_covariance);}struct LinearCostFunction {  template <typename T>  bool operator()(const T* x, const T* y, T* residual) const {    residual[0] = T(10.0) - *x;    residual[1] = T(5.0) - *y;    return true;  }  static CostFunction* Create() {    return new AutoDiffCostFunction<LinearCostFunction, 2, 1, 1>(        new LinearCostFunction);  }};TEST(Covariance, ZeroSizedLocalParameterizationGetCovariance) {  double x = 0.0;  double y = 1.0;  Problem problem;  problem.AddResidualBlock(LinearCostFunction::Create(), nullptr, &x, &y);  problem.SetParameterization(&y, new SubsetParameterization(1, {0}));  // J = [-1 0]  //     [ 0 0]  Covariance::Options options;  options.algorithm_type = DENSE_SVD;  Covariance covariance(options);  vector<pair<const double*, const double*>> covariance_blocks;  covariance_blocks.push_back(std::make_pair(&x, &x));  covariance_blocks.push_back(std::make_pair(&x, &y));  covariance_blocks.push_back(std::make_pair(&y, &x));  covariance_blocks.push_back(std::make_pair(&y, &y));  EXPECT_TRUE(covariance.Compute(covariance_blocks, &problem));  double value = -1;  covariance.GetCovarianceBlock(&x, &x, &value);  EXPECT_NEAR(value, 1.0, std::numeric_limits<double>::epsilon());  value = -1;  covariance.GetCovarianceBlock(&x, &y, &value);  EXPECT_NEAR(value, 0.0, std::numeric_limits<double>::epsilon());  value = -1;  covariance.GetCovarianceBlock(&y, &x, &value);  EXPECT_NEAR(value, 0.0, std::numeric_limits<double>::epsilon());  value = -1;  covariance.GetCovarianceBlock(&y, &y, &value);  EXPECT_NEAR(value, 0.0, std::numeric_limits<double>::epsilon());}TEST(Covariance, ZeroSizedLocalParameterizationGetCovarianceInTangentSpace) {  double x = 0.0;  double y = 1.0;  Problem problem;  problem.AddResidualBlock(LinearCostFunction::Create(), nullptr, &x, &y);  problem.SetParameterization(&y, new SubsetParameterization(1, {0}));  // J = [-1 0]  //     [ 0 0]  Covariance::Options options;  options.algorithm_type = DENSE_SVD;  Covariance covariance(options);  vector<pair<const double*, const double*>> covariance_blocks;  covariance_blocks.push_back(std::make_pair(&x, &x));  covariance_blocks.push_back(std::make_pair(&x, &y));  covariance_blocks.push_back(std::make_pair(&y, &x));  covariance_blocks.push_back(std::make_pair(&y, &y));  EXPECT_TRUE(covariance.Compute(covariance_blocks, &problem));  double value = -1;  covariance.GetCovarianceBlockInTangentSpace(&x, &x, &value);  EXPECT_NEAR(value, 1.0, std::numeric_limits<double>::epsilon());  value = -1;  // The following three calls, should not touch this value, since the  // tangent space is of size zero  covariance.GetCovarianceBlockInTangentSpace(&x, &y, &value);  EXPECT_EQ(value, -1);  covariance.GetCovarianceBlockInTangentSpace(&y, &x, &value);  EXPECT_EQ(value, -1);  covariance.GetCovarianceBlockInTangentSpace(&y, &y, &value);  EXPECT_EQ(value, -1);}class LargeScaleCovarianceTest : public ::testing::Test { protected:  void SetUp() final {    num_parameter_blocks_ = 2000;    parameter_block_size_ = 5;    parameters_.reset(        new double[parameter_block_size_ * num_parameter_blocks_]);    Matrix jacobian(parameter_block_size_, parameter_block_size_);    for (int i = 0; i < num_parameter_blocks_; ++i) {      jacobian.setIdentity();      jacobian *= (i + 1);      double* block_i = parameters_.get() + i * parameter_block_size_;      problem_.AddResidualBlock(          new UnaryCostFunction(              parameter_block_size_, parameter_block_size_, jacobian.data()),          NULL,          block_i);      for (int j = i; j < num_parameter_blocks_; ++j) {        double* block_j = parameters_.get() + j * parameter_block_size_;        all_covariance_blocks_.push_back(make_pair(block_i, block_j));      }    }  }  void ComputeAndCompare(      CovarianceAlgorithmType algorithm_type,      SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,      int num_threads) {    Covariance::Options options;    options.algorithm_type = algorithm_type;    options.sparse_linear_algebra_library_type =        sparse_linear_algebra_library_type;    options.num_threads = num_threads;    Covariance covariance(options);    EXPECT_TRUE(covariance.Compute(all_covariance_blocks_, &problem_));    Matrix expected(parameter_block_size_, parameter_block_size_);    Matrix actual(parameter_block_size_, parameter_block_size_);    const double kTolerance = 1e-16;    for (int i = 0; i < num_parameter_blocks_; ++i) {      expected.setIdentity();      expected /= (i + 1.0) * (i + 1.0);      double* block_i = parameters_.get() + i * parameter_block_size_;      covariance.GetCovarianceBlock(block_i, block_i, actual.data());      EXPECT_NEAR((expected - actual).norm(), 0.0, kTolerance)          << "block: " << i << ", " << i << "\n"          << "expected: \n"          << expected << "\n"          << "actual: \n"          << actual;      expected.setZero();      for (int j = i + 1; j < num_parameter_blocks_; ++j) {        double* block_j = parameters_.get() + j * parameter_block_size_;        covariance.GetCovarianceBlock(block_i, block_j, actual.data());        EXPECT_NEAR((expected - actual).norm(), 0.0, kTolerance)            << "block: " << i << ", " << j << "\n"            << "expected: \n"            << expected << "\n"            << "actual: \n"            << actual;      }    }  }  std::unique_ptr<double[]> parameters_;  int parameter_block_size_;  int num_parameter_blocks_;  Problem problem_;  vector<pair<const double*, const double*>> all_covariance_blocks_;};#if !defined(CERES_NO_SUITESPARSE) && defined(CERES_USE_OPENMP)TEST_F(LargeScaleCovarianceTest, Parallel) {  ComputeAndCompare(SPARSE_QR, SUITE_SPARSE, 4);}#endif  // !defined(CERES_NO_SUITESPARSE) && defined(CERES_USE_OPENMP)}  // namespace internal}  // namespace ceres
 |