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

add stack_matrices to prevent the name conflict with class std::stack

release/4.3a0
Kai Ni 2010-04-02 00:04:11 +00:00
parent 6abb9af0a6
commit 94395249fb
3 changed files with 24 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cpp/BearingRangeFactor.h
View File

@ -40,12 +40,15 @@ namespace gtsam {
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */ /** h(x)-z -> between(z,h(x)) for Rot2 manifold */
Vector evaluateError(const Pose2& pose, const Point2& point, Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
boost::optional<Matrix&> H11 = H1, H21 = H1; Matrix H11, H21, H12, H22;
boost::optional<Matrix&> H12 = H2, H22 = H2; boost::optional<Matrix&> H11_ = H1 ? boost::optional<Matrix&>(H11) : boost::optional<Matrix&>();
Vector e1 = bearing_.evaluateError(pose, point, H11, H12); boost::optional<Matrix&> H21_ = H1 ? boost::optional<Matrix&>(H21) : boost::optional<Matrix&>();
Vector e2 = range_.evaluateError(pose, point, H21, H21); boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>();
if (H1) *H1 = stack(2, &(*H11), &(*H21)); boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
if (H2) *H2 = stack(2, &(*H12), &(*H22)); Vector e1 = bearing_.evaluateError(pose, point, H11_, H12_);
Vector e2 = range_.evaluateError(pose, point, H21_, H22_);
if (H1) *H1 = stack_matrices(H11, H21);
if (H2) *H2 = stack_matrices(H12, H22);
return concatVectors(2, &e1, &e2); return concatVectors(2, &e1, &e2);
} }
@ -53,6 +56,13 @@ namespace gtsam {
inline const std::pair<Rot2, double> measured() const { inline const std::pair<Rot2, double> measured() const {
return concatVectors(2, bearing_.measured(), range_.measured()); return concatVectors(2, bearing_.measured(), range_.measured());
} }
/** return the bearing factor */
const BearingFactor<Config, PoseKey, PointKey>& bearing() const { return bearing_; }
/** return the range factor */
const RangeFactor<Config, PoseKey, PointKey>& range() const { return range_; }
}; // BearingRangeFactor }; // BearingRangeFactor
} // namespace gtsam } // namespace gtsam

5
cpp/Matrix.cpp
View File

@ -742,6 +742,11 @@ Matrix stack(size_t nrMatrices, ...)
return A; return A;
} }
/* ************************************************************************* */
Matrix stack_matrices(const Matrix& A, const Matrix& B) {
return stack(2, &A, &B);
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix collect(const std::vector<const Matrix *>& matrices, size_t m, size_t n) Matrix collect(const std::vector<const Matrix *>& matrices, size_t m, size_t n)
{ {

3
cpp/Matrix.h
View File

@ -287,6 +287,9 @@ Vector backSubstituteLower(const Matrix& L, const Vector& d, bool unit=false);
*/ */
Matrix stack(size_t nrMatrices, ...); Matrix stack(size_t nrMatrices, ...);
/** a shortcut to prevent the name confliction with STL stack */
Matrix stack_matrices(const Matrix& A, const Matrix& B);
/** /**
* create a matrix by concatenating * create a matrix by concatenating
* Given a set of matrices: A1, A2, A3... * Given a set of matrices: A1, A2, A3...