Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

New functionality in linear #1095

Merged
merged 6 commits into from
Feb 12, 2022
Merged

New functionality in linear #1095

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
f278048
VectorValues html
dellaert Feb 9, 2022
fb312f9
likelihood method for GaussianConditionals
dellaert Feb 9, 2022
8dcde7b
Make work with boost 1.74
dellaert Feb 12, 2022
d57e5d5
fix TBB ordering issue
dellaert Feb 12, 2022
9f2e5ab
Fix tiny issues
dellaert Feb 12, 2022
6337241
Add boost version guard
dellaert Feb 12, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions gtsam/base/FastSet.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,9 @@

#pragma once

#if BOOST_VERSION >= 107400
#include <boost/serialization/library_version_type.hpp>
#endif
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/set.hpp>
#include <gtsam/base/FastDefaultAllocator.h>
Expand Down
1 change: 1 addition & 0 deletions gtsam/base/serialization.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include <string>

// includes for standard serialization types
#include <boost/serialization/version.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>
Expand Down
4 changes: 2 additions & 2 deletions gtsam/discrete/DiscreteConditional.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -225,13 +225,13 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(

/* ****************************************************************************/
DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
size_t parent_value) const {
size_t frontal) const {
if (nrFrontals() != 1)
throw std::invalid_argument(
"Single value likelihood can only be invoked on single-variable "
"conditional");
DiscreteValues values;
values.emplace(keys_[0], parent_value);
values.emplace(keys_[0], frontal);
return likelihood(values);
}

Expand Down
2 changes: 1 addition & 1 deletion gtsam/discrete/DiscreteConditional.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -177,7 +177,7 @@ class GTSAM_EXPORT DiscreteConditional
const DiscreteValues& frontalValues) const;

/** Single variable version of likelihood. */
DecisionTreeFactor::shared_ptr likelihood(size_t parent_value) const;
DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const;

/**
* sample
Expand Down
44 changes: 44 additions & 0 deletions gtsam/linear/GaussianConditional.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -224,6 +224,50 @@ namespace gtsam {
}
}

/* ************************************************************************ */
JacobianFactor::shared_ptr GaussianConditional::likelihood(
const VectorValues& frontalValues) const {
// Error is |Rx - (d - Sy - Tz - ...)|^2
// so when we instantiate x (which has to be completely known) we beget:
// |Sy + Tz + ... - (d - Rx)|^2
// The noise model just transfers over!

// Get frontalValues as vector
const Vector x =
frontalValues.vector(KeyVector(beginFrontals(), endFrontals()));

// Copy the augmented Jacobian matrix:
auto newAb = Ab_;

// Restrict view to parent blocks
newAb.firstBlock() += nrFrontals_;

// Update right-hand-side (last column)
auto last = newAb.matrix().cols() - 1;
const auto RR = R().triangularView<Eigen::Upper>();
newAb.matrix().col(last) -= RR * x;

// The keys now do not include the frontal keys:
KeyVector newKeys;
newKeys.reserve(nrParents());
for (auto&& key : parents()) newKeys.push_back(key);

// Hopefully second newAb copy below is optimized out...
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

By this comment, do you mean hopefully the compiler optimizes it out?

Because indeed I think the lines:
auto newAb = Ab_
and
return boost::make_shared<JacobianFactor>(newKeys, newAb, model_);
are both making copies.

Although I guess since the JacobianFactor constructor is copying anyway, I can't think of a functional-style way to avoid the extra copy.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The first one is for sure, but maybe the second one is optimized out? A la RVO?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hmm I don't think RVO can optimize-out a copy into a function call argument, only for a return value out (i.e. shared_ptr won't have extra copy), but ofc I'm not very knowledgable here.

Though I tried making a quick mock-up:
https://godbolt.org/z/Wze8EW5x3

I think the place the copy is actually happening is the JacobianFactor constructor:

  JacobianFactor::JacobianFactor(
    const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) :
  Base(keys), Ab_(augmentedMatrix)

attention: Ab_(matrix)
The const VerticalBlockMatrix & ensures there's not a copy there, and I don't know if the standard allows introspecting that far to check to optimize-out a copy

Well in any case, I don't know the copy can be easily avoided regardless, since newAb has to get modified at some point without changing Ab_.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Note I think if you do Obj(m): m(std::move(m)) {} the move is guaranteed.

return boost::make_shared<JacobianFactor>(newKeys, newAb, model_);
}

/* **************************************************************************/
JacobianFactor::shared_ptr GaussianConditional::likelihood(
const Vector& frontal) const {
if (nrFrontals() != 1)
throw std::invalid_argument(
"GaussianConditional Single value likelihood can only be invoked on "
"single-variable conditional");
VectorValues values;
values.insert(keys_[0], frontal);
return likelihood(values);
}

/* ************************************************************************ */
VectorValues GaussianConditional::sample(const VectorValues& parentsValues,
std::mt19937_64* rng) const {
Expand Down
7 changes: 7 additions & 0 deletions gtsam/linear/GaussianConditional.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -151,6 +151,13 @@ namespace gtsam {
/** Performs transpose backsubstition in place on values */
void solveTransposeInPlace(VectorValues& gy) const;

/** Convert to a likelihood factor by providing value before bar. */
JacobianFactor::shared_ptr likelihood(
const VectorValues& frontalValues) const;

/** Single variable version of likelihood. */
JacobianFactor::shared_ptr likelihood(const Vector& frontal) const;

/**
* Sample from conditional, zero parent version
* Example:
Expand Down
93 changes: 62 additions & 31 deletions gtsam/linear/VectorValues.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ namespace gtsam {
using boost::adaptors::map_values;
using boost::accumulate;

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues::VectorValues(const VectorValues& first, const VectorValues& second)
{
// Merge using predicate for comparing first of pair
Expand All @@ -44,7 +44,7 @@ namespace gtsam {
throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues::VectorValues(const Vector& x, const Dims& dims) {
using Pair = pair<const Key, size_t>;
size_t j = 0;
Expand All @@ -61,7 +61,7 @@ namespace gtsam {
}
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues::VectorValues(const Vector& x, const Scatter& scatter) {
size_t j = 0;
for (const SlotEntry& v : scatter) {
Expand All @@ -74,7 +74,7 @@ namespace gtsam {
}
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::Zero(const VectorValues& other)
{
VectorValues result;
Expand All @@ -87,7 +87,7 @@ namespace gtsam {
return result;
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues::iterator VectorValues::insert(const std::pair<Key, Vector>& key_value) {
std::pair<iterator, bool> result = values_.insert(key_value);
if(!result.second)
Expand All @@ -97,7 +97,7 @@ namespace gtsam {
return result.first;
}

/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::update(const VectorValues& values)
{
iterator hint = begin();
Expand All @@ -115,7 +115,7 @@ namespace gtsam {
}
}

/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::insert(const VectorValues& values)
{
size_t originalSize = size();
Expand All @@ -124,14 +124,14 @@ namespace gtsam {
throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys.");
}

/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::setZero()
{
for(Vector& v: values_ | map_values)
v.setZero();
}

/* ************************************************************************* */
/* ************************************************************************ */
GTSAM_EXPORT ostream& operator<<(ostream& os, const VectorValues& v) {
// Change print depending on whether we are using TBB
#ifdef GTSAM_USE_TBB
Expand All @@ -150,15 +150,15 @@ namespace gtsam {
return os;
}

/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::print(const string& str,
const KeyFormatter& formatter) const {
cout << str << ": " << size() << " elements\n";
cout << key_formatter(formatter) << *this;
cout.flush();
}

/* ************************************************************************* */
/* ************************************************************************ */
bool VectorValues::equals(const VectorValues& x, double tol) const {
if(this->size() != x.size())
return false;
Expand All @@ -170,7 +170,7 @@ namespace gtsam {
return true;
}

/* ************************************************************************* */
/* ************************************************************************ */
Vector VectorValues::vector() const {
// Count dimensions
DenseIndex totalDim = 0;
Expand All @@ -187,7 +187,7 @@ namespace gtsam {
return result;
}

/* ************************************************************************* */
/* ************************************************************************ */
Vector VectorValues::vector(const Dims& keys) const
{
// Count dimensions
Expand All @@ -203,12 +203,12 @@ namespace gtsam {
return result;
}

/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::swap(VectorValues& other) {
this->values_.swap(other.values_);
}

/* ************************************************************************* */
/* ************************************************************************ */
namespace internal
{
bool structureCompareOp(const boost::tuple<VectorValues::value_type,
Expand All @@ -219,14 +219,14 @@ namespace gtsam {
}
}

/* ************************************************************************* */
/* ************************************************************************ */
bool VectorValues::hasSameStructure(const VectorValues other) const
{
return accumulate(combine(*this, other)
| transformed(internal::structureCompareOp), true, logical_and<bool>());
}

/* ************************************************************************* */
/* ************************************************************************ */
double VectorValues::dot(const VectorValues& v) const
{
if(this->size() != v.size())
Expand All @@ -244,12 +244,12 @@ namespace gtsam {
return result;
}

/* ************************************************************************* */
/* ************************************************************************ */
double VectorValues::norm() const {
return std::sqrt(this->squaredNorm());
}

/* ************************************************************************* */
/* ************************************************************************ */
double VectorValues::squaredNorm() const {
double sumSquares = 0.0;
using boost::adaptors::map_values;
Expand All @@ -258,7 +258,7 @@ namespace gtsam {
return sumSquares;
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::operator+(const VectorValues& c) const
{
if(this->size() != c.size())
Expand All @@ -278,13 +278,13 @@ namespace gtsam {
return result;
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::add(const VectorValues& c) const
{
return *this + c;
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::operator+=(const VectorValues& c)
{
if(this->size() != c.size())
Expand All @@ -301,13 +301,13 @@ namespace gtsam {
return *this;
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::addInPlace(const VectorValues& c)
{
return *this += c;
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::addInPlace_(const VectorValues& c)
{
for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) {
Expand All @@ -320,7 +320,7 @@ namespace gtsam {
return *this;
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::operator-(const VectorValues& c) const
{
if(this->size() != c.size())
Expand All @@ -340,13 +340,13 @@ namespace gtsam {
return result;
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::subtract(const VectorValues& c) const
{
return *this - c;
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues operator*(const double a, const VectorValues &v)
{
VectorValues result;
Expand All @@ -359,26 +359,57 @@ namespace gtsam {
return result;
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::scale(const double a) const
{
return a * *this;
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::operator*=(double alpha)
{
for(Vector& v: *this | map_values)
v *= alpha;
return *this;
}

/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::scaleInPlace(double alpha)
{
return *this *= alpha;
}

/* ************************************************************************* */
/* ************************************************************************ */
string VectorValues::html(const KeyFormatter& keyFormatter) const {
stringstream ss;

// Print out preamble.
ss << "<div>\n<table class='VectorValues'>\n <thead>\n";

// Print out header row.
ss << " <tr><th>Variable</th><th>value</th></tr>\n";

// Finish header and start body.
ss << " </thead>\n <tbody>\n";

// Print out all rows.
#ifdef GTSAM_USE_TBB
// TBB uses un-ordered map, so inefficiently order them:
std::map<Key, Vector> ordered;
for (const auto& kv : *this) ordered.emplace(kv);
for (const auto& kv : ordered) {
#else
for (const auto& kv : *this) {
#endif
ss << " <tr>";
ss << "<th>" << keyFormatter(kv.first) << "</th><td>"
<< kv.second.transpose() << "</td>";
ss << "</tr>\n";
}
ss << " </tbody>\n</table>\n</div>";
return ss.str();
}

/* ************************************************************************ */

} // \namespace gtsam
Loading