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

Print Vectors horizontally for easier reading

release/4.3a0
Varun Agrawal 2020-08-31 13:06:32 -04:00
parent 39aeae657e
commit 1cba2dc3e7
5 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Point2.cpp
View File

@ -23,6 +23,12 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point2& p) {
os << p.transpose();
return os;
}
/* ************************************************************************* */ /* ************************************************************************* */
double norm2(const Point2& p, OptionalJacobian<1,2> H) { double norm2(const Point2& p, OptionalJacobian<1,2> H) {
double r = std::sqrt(p.x() * p.x() + p.y() * p.y()); double r = std::sqrt(p.x() * p.x() + p.y() * p.y());

2
gtsam/geometry/Point2.h
View File

@ -26,6 +26,8 @@ namespace gtsam {
/// it is now possible to just typedef Point2 to Vector2 /// it is now possible to just typedef Point2 to Vector2
typedef Vector2 Point2; typedef Vector2 Point2;
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2 &p);
/// Distance of the point from the origin, with Jacobian /// Distance of the point from the origin, with Jacobian
GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);

6
gtsam/geometry/Point3.cpp
View File

@ -22,6 +22,12 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point3& p) {
os << p.transpose();
return os;
}
/* ************************************************************************* */ /* ************************************************************************* */
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) { OptionalJacobian<1, 3> H2) {

2
gtsam/geometry/Point3.h
View File

@ -33,6 +33,8 @@ namespace gtsam {
/// it is now possible to just typedef Point3 to Vector3 /// it is now possible to just typedef Point3 to Vector3
typedef Vector3 Point3; typedef Vector3 Point3;
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3 &p);
// Convenience typedef // Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair; typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);

2
gtsam/navigation/tests/testNavState.cpp
View File

@ -215,7 +215,7 @@ TEST(NavState, Stream)
os << state; os << state;
string expected; string expected;
expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0\n0\n0\nv: 0\n0\n0"; expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0 0 0\nv: 0 0 0";
EXPECT(os.str() == expected); EXPECT(os.str() == expected);
} }