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

Adapted ChartValue so that it can wrap a value to be passed to ExpressionFactor

release/4.3a0
Renaud Dube 2014-10-30 15:59:28 +01:00
parent 18ad04970a
commit 7d8ba565e5
1 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/base/ChartValue.h
View File

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