diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index fb51a55f8..a672d29ee 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -111,6 +111,16 @@ class ChartValue : public GenericValue, public Chart_ { return Chart::local(GenericValue::value(), genericValue2.value()); } + /// Non-virtual version of retract + ChartValue retract(const Vector& delta) const { + return ChartValue(Chart::retract(GenericValue::value(), delta),static_cast(*this)); + } + + /// Non-virtual version of localCoordinates + Vector localCoordinates(const ChartValue& value2) const { + return localCoordinates_(value2); + } + virtual size_t dim() const { return Chart::getDimension(GenericValue::value()); // need functional form here since the dimension may be dynamic } @@ -150,5 +160,13 @@ const Chart& Value::getChart() const { return dynamic_cast(*this); } +/// Convenience function that can be used to make an expression to convert a value to a chart +template +ChartValue convertToChartValue(const T& value, boost::optional::value, traits::dimension::value >& > H=boost::none) { + if (H) { + *H = Eigen::Matrix::value, traits::dimension::value >::Identity(); + } + return ChartValue(value); +} } /* namespace gtsam */