GraphvizFormatting.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2021, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #include <gtsam/nonlinear/Values.h>
21 
22 // TODO(frank): nonlinear should not depend on geometry:
23 #include <gtsam/geometry/Pose2.h>
24 #include <gtsam/geometry/Pose3.h>
25 
26 #include <limits>
27 
28 namespace gtsam {
29 
31  const KeySet& keys) const {
32  Vector2 min;
33  min.x() = std::numeric_limits<double>::infinity();
34  min.y() = std::numeric_limits<double>::infinity();
35  for (const Key& key : keys) {
36  if (values.exists(key)) {
37  std::optional<Vector2> xy = extractPosition(values.at(key));
38  if (xy) {
39  if (xy->x() < min.x()) min.x() = xy->x();
40  if (xy->y() < min.y()) min.y() = xy->y();
41  }
42  }
43  }
44  return min;
45 }
46 
47 std::optional<Vector2> GraphvizFormatting::extractPosition(
48  const Value& value) const {
49  Vector3 t;
50  if (const GenericValue<Pose2>* p =
51  dynamic_cast<const GenericValue<Pose2>*>(&value)) {
52  t << p->value().x(), p->value().y(), 0;
53  } else if (const GenericValue<Vector2>* p =
54  dynamic_cast<const GenericValue<Vector2>*>(&value)) {
55  t << p->value().x(), p->value().y(), 0;
56  } else if (const GenericValue<Vector>* p =
57  dynamic_cast<const GenericValue<Vector>*>(&value)) {
58  if (p->dim() == 2) {
59  const Eigen::Ref<const Vector2> p_2d(p->value());
60  t << p_2d.x(), p_2d.y(), 0;
61  } else if (p->dim() == 3) {
62  const Eigen::Ref<const Vector3> p_3d(p->value());
63  t = p_3d;
64  } else {
65  return {};
66  }
67  } else if (const GenericValue<Pose3>* p =
68  dynamic_cast<const GenericValue<Pose3>*>(&value)) {
69  t = p->value().translation();
70  } else if (const GenericValue<Point3>* p =
71  dynamic_cast<const GenericValue<Point3>*>(&value)) {
72  t = p->value();
73  } else {
74  return {};
75  }
76  double x, y;
77  switch (paperHorizontalAxis) {
78  case X:
79  x = t.x();
80  break;
81  case Y:
82  x = t.y();
83  break;
84  case Z:
85  x = t.z();
86  break;
87  case NEGX:
88  x = -t.x();
89  break;
90  case NEGY:
91  x = -t.y();
92  break;
93  case NEGZ:
94  x = -t.z();
95  break;
96  default:
97  throw std::runtime_error("Invalid enum value");
98  }
99  switch (paperVerticalAxis) {
100  case X:
101  y = t.x();
102  break;
103  case Y:
104  y = t.y();
105  break;
106  case Z:
107  y = t.z();
108  break;
109  case NEGX:
110  y = -t.x();
111  break;
112  case NEGY:
113  y = -t.y();
114  break;
115  case NEGZ:
116  y = -t.z();
117  break;
118  default:
119  throw std::runtime_error("Invalid enum value");
120  }
121  return Vector2(x, y);
122 }
123 
124 std::optional<Vector2> GraphvizFormatting::variablePos(const Values& values,
125  const Vector2& min,
126  Key key) const {
127  if (!values.exists(key)) return DotWriter::variablePos(key);
128  std::optional<Vector2> xy = extractPosition(values.at(key));
129  if (xy) {
130  xy->x() = scale * (xy->x() - min.x());
131  xy->y() = scale * (xy->y() - min.y());
132  }
133  return xy;
134 }
135 
136 std::optional<Vector2> GraphvizFormatting::factorPos(const Vector2& min,
137  size_t i) const {
138  if (factorPositions.size() == 0) return {};
139  auto it = factorPositions.find(i);
140  if (it == factorPositions.end()) return {};
141  auto pos = it->second;
142  return Vector2(scale * (pos.x() - min.x()), scale * (pos.y() - min.y()));
143 }
144 
145 } // namespace gtsam
Pose2.h
2D Pose
gtsam::GraphvizFormatting::scale
double scale
Scale all positions to reduce / increase density.
Definition: GraphvizFormatting.h:40
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
gtsam::GraphvizFormatting::NEGX
@ NEGX
Definition: GraphvizFormatting.h:34
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
xy
Matrix< float, 2, 1 > xy
Definition: LLT_solve.cpp:7
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::GraphvizFormatting::Z
@ Z
Definition: GraphvizFormatting.h:34
gtsam::GraphvizFormatting::findBounds
Vector2 findBounds(const Values &values, const KeySet &keys) const
Definition: GraphvizFormatting.cpp:30
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::FastSet< Key >
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::GraphvizFormatting::paperVerticalAxis
Axis paperVerticalAxis
Definition: GraphvizFormatting.h:38
gtsam::DotWriter::variablePos
std::optional< Vector2 > variablePos(Key key) const
Return variable position or none.
Definition: DotWriter.cpp:79
gtsam::GenericValue
Definition: GenericValue.h:44
gtsam::GraphvizFormatting::variablePos
std::optional< Vector2 > variablePos(const Values &values, const Vector2 &min, Key key) const
Return affinely transformed variable position if it exists.
Definition: GraphvizFormatting.cpp:124
gtsam::GraphvizFormatting::NEGY
@ NEGY
Definition: GraphvizFormatting.h:34
gtsam::GraphvizFormatting::X
@ X
Definition: GraphvizFormatting.h:34
gtsam::Value
Definition: Value.h:39
Vector2
Definition: test_operator_overloading.cpp:18
gtsam::GraphvizFormatting::Y
@ Y
Definition: GraphvizFormatting.h:34
y
Scalar * y
Definition: level1_cplx_impl.h:124
key
const gtsam::Symbol key('X', 0)
Eigen::Ref
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:281
gtsam
traits
Definition: SFMdata.h:40
gtsam::GraphvizFormatting::extractPosition
std::optional< Vector2 > extractPosition(const Value &value) const
Extract a Vector2 from either Vector2, Pose2, Pose3, or Point3.
Definition: GraphvizFormatting.cpp:47
gtsam::Values
Definition: Values.h:65
GraphvizFormatting.h
Graphviz formatter for NonlinearFactorGraph.
p
float * p
Definition: Tutorial_Map_using.cpp:9
min
#define min(a, b)
Definition: datatypes.h:19
gtsam::GraphvizFormatting::paperHorizontalAxis
Axis paperHorizontalAxis
Definition: GraphvizFormatting.h:36
pos
Definition: example-NearestNeighbor.cpp:32
align_3::t
Point2 t(10, 10)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
test_callbacks.value
value
Definition: test_callbacks.py:160
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::DotWriter::factorPositions
std::map< size_t, Vector2 > factorPositions
Definition: DotWriter.h:64
Values.h
A non-templated config holding any types of Manifold-group elements.
Pose3.h
3D Pose
gtsam::GraphvizFormatting::NEGZ
@ NEGZ
Definition: GraphvizFormatting.h:34
gtsam::GraphvizFormatting::factorPos
std::optional< Vector2 > factorPos(const Vector2 &min, size_t i) const
Return affinely transformed factor position if it exists.
Definition: GraphvizFormatting.cpp:136


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:23