123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388 |
- #ifndef CERES_INTERNAL_PARAMETER_BLOCK_H_
- #define CERES_INTERNAL_PARAMETER_BLOCK_H_
- #include <algorithm>
- #include <cstdint>
- #include <cstdlib>
- #include <limits>
- #include <memory>
- #include <string>
- #include <unordered_set>
- #include "ceres/array_utils.h"
- #include "ceres/internal/disable_warnings.h"
- #include "ceres/internal/eigen.h"
- #include "ceres/internal/export.h"
- #include "ceres/manifold.h"
- #include "ceres/stringprintf.h"
- #include "glog/logging.h"
- namespace ceres::internal {
- class ProblemImpl;
- class ResidualBlock;
- class CERES_NO_EXPORT ParameterBlock {
- public:
- using ResidualBlockSet = std::unordered_set<ResidualBlock*>;
-
-
-
- ParameterBlock(double* user_state, int size, int index)
- : user_state_(user_state),
- size_(size),
- state_(user_state),
- index_(index) {}
- ParameterBlock(double* user_state, int size, int index, Manifold* manifold)
- : user_state_(user_state),
- size_(size),
- state_(user_state),
- index_(index) {
- if (manifold != nullptr) {
- SetManifold(manifold);
- }
- }
-
- int Size() const { return size_; }
-
- bool SetState(const double* x) {
- CHECK(x != nullptr) << "Tried to set the state of constant parameter "
- << "with user location " << user_state_;
- CHECK(!IsConstant()) << "Tried to set the state of constant parameter "
- << "with user location " << user_state_;
- state_ = x;
- return UpdatePlusJacobian();
- }
-
-
-
- void GetState(double* x) const {
- if (x != state_) {
- std::copy(state_, state_ + size_, x);
- }
- }
-
- const double* state() const { return state_; }
- const double* user_state() const { return user_state_; }
- double* mutable_user_state() { return user_state_; }
- const Manifold* manifold() const { return manifold_; }
- Manifold* mutable_manifold() { return manifold_; }
-
- void SetConstant() { is_set_constant_ = true; }
- void SetVarying() { is_set_constant_ = false; }
- bool IsConstant() const { return (is_set_constant_ || TangentSize() == 0); }
- double UpperBound(int index) const {
- return (upper_bounds_ ? upper_bounds_[index]
- : std::numeric_limits<double>::max());
- }
- double LowerBound(int index) const {
- return (lower_bounds_ ? lower_bounds_[index]
- : -std::numeric_limits<double>::max());
- }
- bool IsUpperBounded() const { return (upper_bounds_ == nullptr); }
- bool IsLowerBounded() const { return (lower_bounds_ == nullptr); }
-
- int index() const { return index_; }
- void set_index(int index) { index_ = index; }
-
- int state_offset() const { return state_offset_; }
- void set_state_offset(int state_offset) { state_offset_ = state_offset; }
-
- int delta_offset() const { return delta_offset_; }
- void set_delta_offset(int delta_offset) { delta_offset_ = delta_offset; }
-
-
-
-
- const double* PlusJacobian() const { return plus_jacobian_.get(); }
- int TangentSize() const {
- return (manifold_ == nullptr) ? size_ : manifold_->TangentSize();
- }
-
-
- void SetManifold(Manifold* new_manifold) {
-
-
- if (new_manifold == manifold_) {
- return;
- }
- if (new_manifold == nullptr) {
- manifold_ = nullptr;
- plus_jacobian_ = nullptr;
- return;
- }
- CHECK_EQ(new_manifold->AmbientSize(), size_)
- << "The parameter block has size = " << size_
- << " while the manifold has ambient size = "
- << new_manifold->AmbientSize();
- CHECK_GE(new_manifold->TangentSize(), 0)
- << "Invalid Manifold. Manifolds must have a "
- << "non-negative dimensional tangent space.";
- manifold_ = new_manifold;
- plus_jacobian_ = std::make_unique<double[]>(manifold_->AmbientSize() *
- manifold_->TangentSize());
- CHECK(UpdatePlusJacobian())
- << "Manifold::PlusJacobian computation failed for x: "
- << ConstVectorRef(state_, Size()).transpose();
- }
- void SetUpperBound(int index, double upper_bound) {
- CHECK_LT(index, size_);
- if (upper_bound >= std::numeric_limits<double>::max() && !upper_bounds_) {
- return;
- }
- if (!upper_bounds_) {
- upper_bounds_ = std::make_unique<double[]>(size_);
- std::fill(upper_bounds_.get(),
- upper_bounds_.get() + size_,
- std::numeric_limits<double>::max());
- }
- upper_bounds_[index] = upper_bound;
- }
- void SetLowerBound(int index, double lower_bound) {
- CHECK_LT(index, size_);
- if (lower_bound <= -std::numeric_limits<double>::max() && !lower_bounds_) {
- return;
- }
- if (!lower_bounds_) {
- lower_bounds_ = std::make_unique<double[]>(size_);
- std::fill(lower_bounds_.get(),
- lower_bounds_.get() + size_,
- -std::numeric_limits<double>::max());
- }
- lower_bounds_[index] = lower_bound;
- }
-
-
-
- bool Plus(const double* x, const double* delta, double* x_plus_delta) {
- if (manifold_ != nullptr) {
- if (!manifold_->Plus(x, delta, x_plus_delta)) {
- return false;
- }
- } else {
- VectorRef(x_plus_delta, size_) =
- ConstVectorRef(x, size_) + ConstVectorRef(delta, size_);
- }
-
- if (lower_bounds_.get() != nullptr) {
- for (int i = 0; i < size_; ++i) {
- x_plus_delta[i] = std::max(x_plus_delta[i], lower_bounds_[i]);
- }
- }
- if (upper_bounds_.get() != nullptr) {
- for (int i = 0; i < size_; ++i) {
- x_plus_delta[i] = std::min(x_plus_delta[i], upper_bounds_[i]);
- }
- }
- return true;
- }
- std::string ToString() const {
- return StringPrintf(
- "{ this=%p, user_state=%p, state=%p, size=%d, "
- "constant=%d, index=%d, state_offset=%d, "
- "delta_offset=%d }",
- this,
- user_state_,
- state_,
- size_,
- is_set_constant_,
- index_,
- state_offset_,
- delta_offset_);
- }
- void EnableResidualBlockDependencies() {
- CHECK(residual_blocks_.get() == nullptr)
- << "Ceres bug: There is already a residual block collection "
- << "for parameter block: " << ToString();
- residual_blocks_ = std::make_unique<ResidualBlockSet>();
- }
- void AddResidualBlock(ResidualBlock* residual_block) {
- CHECK(residual_blocks_.get() != nullptr)
- << "Ceres bug: The residual block collection is null for parameter "
- << "block: " << ToString();
- residual_blocks_->insert(residual_block);
- }
- void RemoveResidualBlock(ResidualBlock* residual_block) {
- CHECK(residual_blocks_.get() != nullptr)
- << "Ceres bug: The residual block collection is null for parameter "
- << "block: " << ToString();
- CHECK(residual_blocks_->find(residual_block) != residual_blocks_->end())
- << "Ceres bug: Missing residual for parameter block: " << ToString();
- residual_blocks_->erase(residual_block);
- }
-
-
- ResidualBlockSet* mutable_residual_blocks() { return residual_blocks_.get(); }
- double LowerBoundForParameter(int index) const {
- if (lower_bounds_.get() == nullptr) {
- return -std::numeric_limits<double>::max();
- } else {
- return lower_bounds_[index];
- }
- }
- double UpperBoundForParameter(int index) const {
- if (upper_bounds_.get() == nullptr) {
- return std::numeric_limits<double>::max();
- } else {
- return upper_bounds_[index];
- }
- }
- private:
- bool UpdatePlusJacobian() {
- if (manifold_ == nullptr) {
- return true;
- }
-
-
-
- const int jacobian_size = Size() * TangentSize();
- InvalidateArray(jacobian_size, plus_jacobian_.get());
- if (!manifold_->PlusJacobian(state_, plus_jacobian_.get())) {
- LOG(WARNING) << "Manifold::PlusJacobian computation failed"
- "for x: "
- << ConstVectorRef(state_, Size()).transpose();
- return false;
- }
- if (!IsArrayValid(jacobian_size, plus_jacobian_.get())) {
- LOG(WARNING) << "Manifold::PlusJacobian computation returned "
- << "an invalid matrix for x: "
- << ConstVectorRef(state_, Size()).transpose()
- << "\n Jacobian matrix : "
- << ConstMatrixRef(
- plus_jacobian_.get(), Size(), TangentSize());
- return false;
- }
- return true;
- }
- double* user_state_ = nullptr;
- int size_ = -1;
- bool is_set_constant_ = false;
- Manifold* manifold_ = nullptr;
-
-
-
-
- mutable const double* state_ = nullptr;
- mutable std::unique_ptr<double[]> plus_jacobian_;
-
-
- int index_ = -1;
-
- int state_offset_ = -1;
-
- int delta_offset_ = -1;
-
- std::unique_ptr<ResidualBlockSet> residual_blocks_;
-
-
-
-
-
-
-
-
-
-
-
- std::unique_ptr<double[]> upper_bounds_;
- std::unique_ptr<double[]> lower_bounds_;
-
- friend class ProblemImpl;
- };
- }
- #include "ceres/internal/reenable_warnings.h"
- #endif
|