residual_block_utils.cc 5.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144
  1. // Ceres Solver - A fast non-linear least squares minimizer
  2. // Copyright 2023 Google Inc. All rights reserved.
  3. // http://ceres-solver.org/
  4. //
  5. // Redistribution and use in source and binary forms, with or without
  6. // modification, are permitted provided that the following conditions are met:
  7. //
  8. // * Redistributions of source code must retain the above copyright notice,
  9. // this list of conditions and the following disclaimer.
  10. // * Redistributions in binary form must reproduce the above copyright notice,
  11. // this list of conditions and the following disclaimer in the documentation
  12. // and/or other materials provided with the distribution.
  13. // * Neither the name of Google Inc. nor the names of its contributors may be
  14. // used to endorse or promote products derived from this software without
  15. // specific prior written permission.
  16. //
  17. // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  18. // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19. // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20. // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  21. // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  22. // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  23. // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  24. // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  25. // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  26. // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  27. // POSSIBILITY OF SUCH DAMAGE.
  28. //
  29. // Author: sameeragarwal@google.com (Sameer Agarwal)
  30. #include "ceres/residual_block_utils.h"
  31. #include <cmath>
  32. #include <cstddef>
  33. #include <limits>
  34. #include <string>
  35. #include "ceres/array_utils.h"
  36. #include "ceres/internal/eigen.h"
  37. #include "ceres/internal/export.h"
  38. #include "ceres/parameter_block.h"
  39. #include "ceres/residual_block.h"
  40. #include "ceres/stringprintf.h"
  41. #include "glog/logging.h"
  42. namespace ceres::internal {
  43. void InvalidateEvaluation(const ResidualBlock& block,
  44. double* cost,
  45. double* residuals,
  46. double** jacobians) {
  47. const int num_parameter_blocks = block.NumParameterBlocks();
  48. const int num_residuals = block.NumResiduals();
  49. InvalidateArray(1, cost);
  50. InvalidateArray(num_residuals, residuals);
  51. if (jacobians != nullptr) {
  52. for (int i = 0; i < num_parameter_blocks; ++i) {
  53. const int parameter_block_size = block.parameter_blocks()[i]->Size();
  54. InvalidateArray(num_residuals * parameter_block_size, jacobians[i]);
  55. }
  56. }
  57. }
  58. std::string EvaluationToString(const ResidualBlock& block,
  59. double const* const* parameters,
  60. double* cost,
  61. double* residuals,
  62. double** jacobians) {
  63. CHECK(cost != nullptr);
  64. CHECK(residuals != nullptr);
  65. const int num_parameter_blocks = block.NumParameterBlocks();
  66. const int num_residuals = block.NumResiduals();
  67. std::string result = "";
  68. // clang-format off
  69. StringAppendF(&result,
  70. "Residual Block size: %d parameter blocks x %d residuals\n\n",
  71. num_parameter_blocks, num_residuals);
  72. result +=
  73. "For each parameter block, the value of the parameters are printed in the first column \n" // NOLINT
  74. "and the value of the jacobian under the corresponding residual. If a ParameterBlock was \n" // NOLINT
  75. "held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry \n" // NOLINT
  76. "of the Jacobian/residual array was requested but was not written to by user code, it is \n" // NOLINT
  77. "indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating \n" // NOLINT
  78. "to Inf or NaN is also an error. \n\n"; // NOLINT
  79. // clang-format on
  80. std::string space = "Residuals: ";
  81. result += space;
  82. AppendArrayToString(num_residuals, residuals, &result);
  83. StringAppendF(&result, "\n\n");
  84. for (int i = 0; i < num_parameter_blocks; ++i) {
  85. const int parameter_block_size = block.parameter_blocks()[i]->Size();
  86. StringAppendF(
  87. &result, "Parameter Block %d, size: %d\n", i, parameter_block_size);
  88. StringAppendF(&result, "\n");
  89. for (int j = 0; j < parameter_block_size; ++j) {
  90. AppendArrayToString(1, parameters[i] + j, &result);
  91. StringAppendF(&result, "| ");
  92. for (int k = 0; k < num_residuals; ++k) {
  93. AppendArrayToString(1,
  94. (jacobians != nullptr && jacobians[i] != nullptr)
  95. ? jacobians[i] + k * parameter_block_size + j
  96. : nullptr,
  97. &result);
  98. }
  99. StringAppendF(&result, "\n");
  100. }
  101. StringAppendF(&result, "\n");
  102. }
  103. StringAppendF(&result, "\n");
  104. return result;
  105. }
  106. // TODO(sameeragarwal) Check cost value validness here
  107. // Cost value is a part of evaluation but not checked here since according to
  108. // residual_block.cc cost is not valid at the time this method is called
  109. bool IsEvaluationValid(const ResidualBlock& block,
  110. double const* const* /*parameters*/,
  111. double* residuals,
  112. double** jacobians) {
  113. const int num_parameter_blocks = block.NumParameterBlocks();
  114. const int num_residuals = block.NumResiduals();
  115. if (!IsArrayValid(num_residuals, residuals)) {
  116. return false;
  117. }
  118. if (jacobians != nullptr) {
  119. for (int i = 0; i < num_parameter_blocks; ++i) {
  120. const int parameter_block_size = block.parameter_blocks()[i]->Size();
  121. if (!IsArrayValid(num_residuals * parameter_block_size, jacobians[i])) {
  122. return false;
  123. }
  124. }
  125. }
  126. return true;
  127. }
  128. } // namespace ceres::internal