12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

split HybridValues into .h and .cpp

release/4.3a0
Varun Agrawal 2024-08-25 13:51:14 -04:00
parent 88c2ad55be
commit b54ed7209e
2 changed files with 196 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

167
gtsam/hybrid/HybridValues.cpp Normal file
View File

@ -0,0 +1,167 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file HybridValues.cpp
* @author Varun Agrawal
* @date August 2024
*/
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/Values.h>
namespace gtsam {
/* ************************************************************************* */
HybridValues::HybridValues(const VectorValues& cv, const DiscreteValues& dv)
: continuous_(cv), discrete_(dv) {}
/* ************************************************************************* */
HybridValues::HybridValues(const VectorValues& cv, const DiscreteValues& dv,
const Values& v)
: continuous_(cv), discrete_(dv), nonlinear_(v) {}
/* ************************************************************************* */
void HybridValues::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::cout << s << ": \n";
continuous_.print(" Continuous",
keyFormatter); // print continuous components
discrete_.print(" Discrete", keyFormatter); // print discrete components
}
/* ************************************************************************* */
bool HybridValues::equals(const HybridValues& other, double tol) const {
return continuous_.equals(other.continuous_, tol) &&
discrete_.equals(other.discrete_, tol);
}
/* ************************************************************************* */
const VectorValues& HybridValues::continuous() const { return continuous_; }
/* ************************************************************************* */
const DiscreteValues& HybridValues::discrete() const { return discrete_; }
/* ************************************************************************* */
const Values& HybridValues::nonlinear() const { return nonlinear_; }
/* ************************************************************************* */
bool HybridValues::existsVector(Key j) { return continuous_.exists(j); }
/* ************************************************************************* */
bool HybridValues::existsDiscrete(Key j) {
return (discrete_.find(j) != discrete_.end());
}
/* ************************************************************************* */
bool HybridValues::existsNonlinear(Key j) { return nonlinear_.exists(j); }
/* ************************************************************************* */
bool HybridValues::exists(Key j) {
return existsVector(j) || existsDiscrete(j) || existsNonlinear(j);
}
/* ************************************************************************* */
HybridValues HybridValues::retract(const VectorValues& delta) const {
HybridValues updated(continuous_, discrete_, nonlinear_.retract(delta));
return updated;
}
/* ************************************************************************* */
void HybridValues::insert(Key j, const Vector& value) {
continuous_.insert(j, value);
}
/* ************************************************************************* */
void HybridValues::insert(Key j, size_t value) { discrete_[j] = value; }
/* ************************************************************************* */
void HybridValues::insert_or_assign(Key j, const Vector& value) {
continuous_.insert_or_assign(j, value);
}
/* ************************************************************************* */
void HybridValues::insert_or_assign(Key j, size_t value) {
discrete_[j] = value;
}
/* ************************************************************************* */
HybridValues& HybridValues::insert(const VectorValues& values) {
continuous_.insert(values);
return *this;
}
/* ************************************************************************* */
HybridValues& HybridValues::insert(const DiscreteValues& values) {
discrete_.insert(values);
return *this;
}
/* ************************************************************************* */
HybridValues& HybridValues::insert(const Values& values) {
nonlinear_.insert(values);
return *this;
}
/* ************************************************************************* */
HybridValues& HybridValues::insert(const HybridValues& values) {
continuous_.insert(values.continuous());
discrete_.insert(values.discrete());
nonlinear_.insert(values.nonlinear());
return *this;
}
/* ************************************************************************* */
Vector& HybridValues::at(Key j) { return continuous_.at(j); }
/* ************************************************************************* */
size_t& HybridValues::atDiscrete(Key j) { return discrete_.at(j); }
/* ************************************************************************* */
HybridValues& HybridValues::update(const VectorValues& values) {
continuous_.update(values);
return *this;
}
/* ************************************************************************* */
HybridValues& HybridValues::update(const DiscreteValues& values) {
discrete_.update(values);
return *this;
}
/* ************************************************************************* */
HybridValues& HybridValues::update(const HybridValues& values) {
continuous_.update(values.continuous());
discrete_.update(values.discrete());
return *this;
}
/* ************************************************************************* */
VectorValues HybridValues::continuousSubset(const KeyVector& keys) const {
VectorValues measurements;
for (const auto& key : keys) {
measurements.insert(key, continuous_.at(key));
}
return measurements;
}
/* ************************************************************************* */
std::string HybridValues::html(const KeyFormatter& keyFormatter) const {
std::stringstream ss;
ss << this->continuous_.html(keyFormatter);
ss << this->discrete_.html(keyFormatter);
return ss.str();
}
} // namespace gtsam

106
gtsam/hybrid/HybridValues.h
View File

@ -18,8 +18,6 @@
#pragma once
#include <gtsam/discrete/Assignment.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/VectorValues.h>
@ -55,13 +53,11 @@ class GTSAM_EXPORT HybridValues {
HybridValues() = default;
/// Construct from DiscreteValues and VectorValues.
HybridValues(const VectorValues& cv, const DiscreteValues& dv)
: continuous_(cv), discrete_(dv) {}
HybridValues(const VectorValues& cv, const DiscreteValues& dv);
/// Construct from all values types.
HybridValues(const VectorValues& cv, const DiscreteValues& dv,
const Values& v)
: continuous_(cv), discrete_(dv), nonlinear_(v) {}
const Values& v);
/// @}
/// @name Testable
@ -69,144 +65,105 @@ class GTSAM_EXPORT HybridValues {
/// print required by Testable for unit testing
void print(const std::string& s = "HybridValues",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << ": \n";
continuous_.print(" Continuous",
keyFormatter); // print continuous components
discrete_.print(" Discrete", keyFormatter); // print discrete components
}
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// equals required by Testable for unit testing
bool equals(const HybridValues& other, double tol = 1e-9) const {
return continuous_.equals(other.continuous_, tol) &&
discrete_.equals(other.discrete_, tol);
}
bool equals(const HybridValues& other, double tol = 1e-9) const;
/// @}
/// @name Interface
/// @{
/// Return the multi-dimensional vector values.
const VectorValues& continuous() const { return continuous_; }
const VectorValues& continuous() const;
/// Return the discrete values.
const DiscreteValues& discrete() const { return discrete_; }
const DiscreteValues& discrete() const;
/// Return the nonlinear values.
const Values& nonlinear() const { return nonlinear_; }
const Values& nonlinear() const;
/// Check whether a variable with key \c j exists in VectorValues.
bool existsVector(Key j) { return continuous_.exists(j); }
bool existsVector(Key j);
/// Check whether a variable with key \c j exists in DiscreteValues.
bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); }
bool existsDiscrete(Key j);
/// Check whether a variable with key \c j exists in values.
bool existsNonlinear(Key j) { return nonlinear_.exists(j); }
bool existsNonlinear(Key j);
/// Check whether a variable with key \c j exists.
bool exists(Key j) {
return existsVector(j) || existsDiscrete(j) || existsNonlinear(j);
}
bool exists(Key j);
/** Add a delta config to current config and returns a new config */
HybridValues retract(const VectorValues& delta) const;
/** Insert a vector \c value with key \c j. Throws an invalid_argument
* exception if the key \c j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated. */
void insert(Key j, const Vector& value) { continuous_.insert(j, value); }
void insert(Key j, const Vector& value);
/** Insert a discrete \c value with key \c j. Replaces the existing value if
* the key \c j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated. */
void insert(Key j, size_t value) { discrete_[j] = value; }
void insert(Key j, size_t value);
/// insert_or_assign() , similar to Values.h
void insert_or_assign(Key j, const Vector& value) {
continuous_.insert_or_assign(j, value);
}
void insert_or_assign(Key j, const Vector& value);
/// insert_or_assign() , similar to Values.h
void insert_or_assign(Key j, size_t value) { discrete_[j] = value; }
void insert_or_assign(Key j, size_t value);
/** Insert all continuous values from \c values. Throws an invalid_argument
* exception if any keys to be inserted are already used. */
HybridValues& insert(const VectorValues& values) {
continuous_.insert(values);
return *this;
}
HybridValues& insert(const VectorValues& values);
/** Insert all discrete values from \c values. Throws an invalid_argument
* exception if any keys to be inserted are already used. */
HybridValues& insert(const DiscreteValues& values) {
discrete_.insert(values);
return *this;
}
HybridValues& insert(const DiscreteValues& values);
/** Insert all values from \c values. Throws an invalid_argument
* exception if any keys to be inserted are already used. */
HybridValues& insert(const Values& values) {
nonlinear_.insert(values);
return *this;
}
HybridValues& insert(const Values& values);
/** Insert all values from \c values. Throws an invalid_argument exception if
* any keys to be inserted are already used. */
HybridValues& insert(const HybridValues& values) {
continuous_.insert(values.continuous());
discrete_.insert(values.discrete());
nonlinear_.insert(values.nonlinear());
return *this;
}
HybridValues& insert(const HybridValues& values);
/**
* Read/write access to the vector value with key \c j, throws
* std::out_of_range if \c j does not exist.
*/
Vector& at(Key j) { return continuous_.at(j); }
Vector& at(Key j);
/**
* Read/write access to the discrete value with key \c j, throws
* std::out_of_range if \c j does not exist.
*/
size_t& atDiscrete(Key j) { return discrete_.at(j); }
size_t& atDiscrete(Key j);
/** For all key/value pairs in \c values, replace continuous values with
* corresponding keys in this object with those in \c values. Throws
* std::out_of_range if any keys in \c values are not present in this object.
*/
HybridValues& update(const VectorValues& values) {
continuous_.update(values);
return *this;
}
HybridValues& update(const VectorValues& values);
/** For all key/value pairs in \c values, replace discrete values with
* corresponding keys in this object with those in \c values. Throws
* std::out_of_range if any keys in \c values are not present in this object.
*/
HybridValues& update(const DiscreteValues& values) {
discrete_.update(values);
return *this;
}
HybridValues& update(const DiscreteValues& values);
/** For all key/value pairs in \c values, replace all values with
* corresponding keys in this object with those in \c values. Throws
* std::out_of_range if any keys in \c values are not present in this object.
*/
HybridValues& update(const HybridValues& values) {
continuous_.update(values.continuous());
discrete_.update(values.discrete());
return *this;
}
HybridValues& update(const HybridValues& values);
/// Extract continuous values with given keys.
VectorValues continuousSubset(const KeyVector& keys) const {
VectorValues measurements;
for (const auto& key : keys) {
measurements.insert(key, continuous_.at(key));
}
return measurements;
}
VectorValues continuousSubset(const KeyVector& keys) const;
/// @}
/// @name Wrapper support
@ -219,12 +176,7 @@ class GTSAM_EXPORT HybridValues {
* @return string html output.
*/
std::string html(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::stringstream ss;
ss << this->continuous_.html(keyFormatter);
ss << this->discrete_.html(keyFormatter);
return ss.str();
}
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
};