diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 12cfc706d..95ff66e68 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -49,14 +49,14 @@ namespace gtsam { CalibratedCamera() {} /// construct with pose - CalibratedCamera(const Pose3& pose); + explicit CalibratedCamera(const Pose3& pose); /// @} /// @name Advanced Constructors /// @{ /// construct from vector - CalibratedCamera(const Vector &v); + explicit CalibratedCamera(const Vector &v); /// @} /// @name Testable diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index d00f0fd90..8aee12064 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -51,7 +51,7 @@ namespace gtsam { PinholeCamera() {} /** constructor with pose */ - PinholeCamera(const Pose3& pose):pose_(pose){} + explicit PinholeCamera(const Pose3& pose):pose_(pose){} /** constructor with pose and calibration */ PinholeCamera(const Pose3& pose, const Calibration& k):pose_(pose),k_(k) {} @@ -63,7 +63,7 @@ namespace gtsam { /// @name Advanced Constructors /// @{ - PinholeCamera(const Vector &v){ + explicit PinholeCamera(const Vector &v){ pose_ = Pose3::Expmap(v.head(Pose3::Dim())); if ( v.size() > Pose3::Dim()) { k_ = Calibration(v.tail(Calibration::Dim())); diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 7410ddade..c79d82a18 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -40,23 +40,30 @@ namespace gtsam { ValueCloneAllocator() {} }; +#if 0 /* ************************************************************************* */ class ValueAutomaticCasting { const Symbol& key_; const Value& value_; + public: ValueAutomaticCasting(const Symbol& key, const Value& value) : key_(key), value_(value) {} template - operator const ValueType& () const { + class ConvertibleToValue : public ValueType { + }; + + template + operator const ConvertibleToValue& () const { // Check the type and throw exception if incorrect if(typeid(ValueType) != typeid(value_)) throw ValuesIncorrectType(key_, typeid(ValueType), typeid(value_)); // We have already checked the type, so do a "blind" static_cast, not dynamic_cast - return static_cast(value_); + return static_cast&>(value_); } }; +#endif /* ************************************************************************* */ template @@ -76,6 +83,7 @@ namespace gtsam { return static_cast(*item->second); } +#if 0 /* ************************************************************************* */ inline ValueAutomaticCasting Values::at(const Symbol& j) const { // Find the item @@ -87,6 +95,7 @@ namespace gtsam { return ValueAutomaticCasting(item->first, *item->second); } +#endif /* ************************************************************************* */ template @@ -98,10 +107,12 @@ namespace gtsam { return at(symbol); } +#if 0 /* ************************************************************************* */ inline ValueAutomaticCasting Values::operator[](const Symbol& j) const { return at(j); } +#endif /* ************************************************************************* */ template diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index e2a69d622..61e11ca4e 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -139,6 +139,7 @@ namespace gtsam { template const ValueType& at(const Symbol& j) const; +#if 0 /** Retrieve a variable by key \c j. This non-templated version returns a * special ValueAutomaticCasting object that may be assigned to the proper * value. @@ -147,6 +148,7 @@ namespace gtsam { * of the proper type. */ ValueAutomaticCasting at(const Symbol& j) const; +#endif /** Retrieve a variable using a special key (typically TypedSymbol), which * contains the type of the value associated with the key, and which must @@ -163,8 +165,10 @@ namespace gtsam { const typename TypedKey::Value& operator[](const TypedKey& j) const { return at(j); } +#if 0 /** operator[] syntax for at(const Symbol& j) */ ValueAutomaticCasting operator[](const Symbol& j) const; +#endif /** Check if a value exists with key \c j. See exists<>(const Symbol& j) * and exists(const TypedKey& j) for versions that return the value if it diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 42f42e429..13ae33cfc 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -59,7 +59,7 @@ struct BoundingConstraint1: public NonlinearFactor1 { /** active when constraint *NOT* met */ bool active(const Values& c) const { // note: still active at equality to avoid zigzagging - double x = value(c[this->key()]); + double x = value(c.at(this->key())); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } @@ -127,7 +127,7 @@ struct BoundingConstraint2: public NonlinearFactor2 { /** active when constraint *NOT* met */ bool active(const Values& c) const { // note: still active at equality to avoid zigzagging - double x = value(c[this->key1()], c[this->key2()]); + double x = value(c.at(this->key1()), c.at(this->key2())); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 4fa815cdc..9eb6b3c3c 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -50,8 +50,7 @@ TEST(Pose3Graph, optimizeCircle) { // Create a hexagon of poses double radius = 10; Values hexagon = pose3SLAM::circle(6,radius); -// Pose3 gT0 = hexagon[PoseKey(0)], gT1 = hexagon[PoseKey(1)]; // FAIL: cannot cast ValueAutomaticCasting - Pose3 gT0 = hexagon.at(PoseKey(0)), gT1 = hexagon.at(PoseKey(1)); // Works + Pose3 gT0 = hexagon.at(PoseKey(0)), gT1 = hexagon.at(PoseKey(1)); // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new Pose3Graph); diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 532305f24..0b4b222be 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -181,7 +181,7 @@ public: /** Vector of errors, whitened ! */ Vector whitenedError(const Values& c) const { - return QInvSqrt(c[key1()])*unwhitenedError(c); + return QInvSqrt(c.at(key1()))*unwhitenedError(c); } /** @@ -190,8 +190,8 @@ public: * Hence b = z - h(x1,x2) = - error_vector(x) */ boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { - const X1& x1 = c[key1()]; - const X2& x2 = c[key2()]; + const X1& x1 = c.at(key1()); + const X2& x2 = c.at(key2()); Matrix A1, A2; Vector b = -evaluateError(x1, x2, A1, A2); const Index var1 = ordering[key1()], var2 = ordering[key2()]; @@ -318,7 +318,7 @@ public: /** Vector of errors, whitened ! */ Vector whitenedError(const Values& c) const { - return RInvSqrt(c[key()])*unwhitenedError(c); + return RInvSqrt(c.at(key()))*unwhitenedError(c); } /** @@ -327,7 +327,7 @@ public: * Hence b = z - h(x1) = - error_vector(x) */ boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { - const X& x1 = c[key()]; + const X& x1 = c.at(key()); Matrix A1; Vector b = -evaluateError(x1, A1); const Index var1 = ordering[key()];