mirror of
https://github.com/ceres-solver/ceres-solver.git
synced 2026-08-30 00:50:37 +08:00
Sameer Agarwal
4bf3868bec
The schur eliminator treats rows with e blocks and row with no e blocks separately. The template specialization logic only applies to the rows with e blocks. So, in cases where the rows with e-blocks have a fixed size f-block but the rows without e-blocks have f-blocks of varying sizes, DetectStructure will return a static f-block size, but we need to be careful that we do not blindly use that static f-block size everywhere. This patch fixes a bug where such care was not being taken, where it was assumed that the static f-block size could be assumed for all f-block sizes. A new test is added, which triggers an exception in debug mode. In release mode this error does not present itself, due to a peculiarity of the way Eigen works. Thanks to Werner Trobin for reporting this bug. Change-Id: I8ae7aabf8eed8c3f9cf74b6c74d632ba44f82581
114 lines
4.5 KiB
C++
114 lines
4.5 KiB
C++
// 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/detect_structure.h"
|
|
#include "ceres/internal/eigen.h"
|
|
#include "glog/logging.h"
|
|
|
|
namespace ceres {
|
|
namespace internal {
|
|
|
|
void DetectStructure(const CompressedRowBlockStructure& bs,
|
|
const int num_eliminate_blocks,
|
|
int* row_block_size,
|
|
int* e_block_size,
|
|
int* f_block_size) {
|
|
const int num_row_blocks = bs.rows.size();
|
|
*row_block_size = 0;
|
|
*e_block_size = 0;
|
|
*f_block_size = 0;
|
|
|
|
// Iterate over row blocks of the matrix, checking if row_block,
|
|
// e_block or f_block sizes remain constant.
|
|
for (int r = 0; r < num_row_blocks; ++r) {
|
|
const CompressedRow& row = bs.rows[r];
|
|
// We do not care about the sizes of the blocks in rows which do
|
|
// not contain e_blocks.
|
|
if (row.cells.front().block_id >= num_eliminate_blocks) {
|
|
break;
|
|
}
|
|
|
|
if (*row_block_size == 0) {
|
|
*row_block_size = row.block.size;
|
|
} else if (*row_block_size != Eigen::Dynamic &&
|
|
*row_block_size != row.block.size) {
|
|
VLOG(2) << "Dynamic row block size because the block size changed from "
|
|
<< *row_block_size << " to "
|
|
<< row.block.size;
|
|
*row_block_size = Eigen::Dynamic;
|
|
}
|
|
|
|
const int e_block_id = row.cells.front().block_id;
|
|
if (*e_block_size == 0) {
|
|
*e_block_size = bs.cols[e_block_id].size;
|
|
} else if (*e_block_size != Eigen::Dynamic &&
|
|
*e_block_size != bs.cols[e_block_id].size) {
|
|
VLOG(2) << "Dynamic e block size because the block size changed from "
|
|
<< *e_block_size << " to "
|
|
<< bs.cols[e_block_id].size;
|
|
*e_block_size = Eigen::Dynamic;
|
|
}
|
|
|
|
if (*f_block_size == 0) {
|
|
if (row.cells.size() > 1) {
|
|
const int f_block_id = row.cells[1].block_id;
|
|
*f_block_size = bs.cols[f_block_id].size;
|
|
}
|
|
} else if (*f_block_size != Eigen::Dynamic) {
|
|
for (int c = 1; c < row.cells.size(); ++c) {
|
|
if (*f_block_size != bs.cols[row.cells[c].block_id].size) {
|
|
VLOG(2) << "Dynamic f block size because the block size "
|
|
<< "changed from " << *f_block_size << " to "
|
|
<< bs.cols[row.cells[c].block_id].size;
|
|
*f_block_size = Eigen::Dynamic;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
const bool is_everything_dynamic = (*row_block_size == Eigen::Dynamic &&
|
|
*e_block_size == Eigen::Dynamic &&
|
|
*f_block_size == Eigen::Dynamic);
|
|
if (is_everything_dynamic) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
CHECK_NE(*row_block_size, 0) << "No rows found";
|
|
CHECK_NE(*e_block_size, 0) << "No e type blocks found";
|
|
VLOG(1) << "Schur complement static structure <"
|
|
<< *row_block_size << ","
|
|
<< *e_block_size << ","
|
|
<< *f_block_size << ">.";
|
|
}
|
|
|
|
} // namespace internal
|
|
} // namespace ceres
|