Skip to content

Commit 80c79cd

Browse files
pbustospbustos
authored
Merge pull request #11 from alfiTH/development
Fix numpy >=2.0 compatibility, add Virtual RT and enhance graph visualization features
2 parents 27fadad + 6353e3c commit 80c79cd

12 files changed

Lines changed: 2691 additions & 195 deletions

File tree

‎api/dsr_inner_eigen_api.cpp‎

Lines changed: 42 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -18,9 +18,11 @@ InnerEigenAPI::InnerEigenAPI(DSR::DSRGraph *G_)
1818
////// TRANSFORMATION MATRIX
1919
////////////////////////////////////////////////////////////////////////////////////////
2020

21-
std::optional<Mat::RTMat> InnerEigenAPI::get_transformation_matrix(const std::string &dest, const std::string &orig, std::uint64_t timestamp)
21+
std::optional<Mat::RTMat> InnerEigenAPI::get_transformation_matrix(const std::string &dest, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type)
2222
{
23-
KeyTransform key = std::make_tuple(dest, orig);
23+
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
24+
return {};
25+
KeyTransform key = std::make_tuple(dest, orig, edge_type);
2426
if( auto it = cache.find(key) ; it != cache.end())
2527
return it->second;
2628
else
@@ -48,10 +50,10 @@ std::optional<Mat::RTMat> InnerEigenAPI::get_transformation_matrix(const std::st
4850
auto p_node = G->get_parent_node(a);
4951
if( not p_node.has_value())
5052
break;
51-
auto edge_rt = rt->get_edge_RT(p_node.value(), a.id());
53+
auto edge_rt = rt->get_edge_RT(p_node.value(), a.id(), edge_type);
5254
if (not edge_rt.has_value())
5355
{
54-
qWarning() << __FUNCTION__ << ":"<<__LINE__<< " Cannot find RT edge between Parent (" << QString::fromStdString(p_node->name()) << ", " << p_node->id() <<") and son (" << QString::fromStdString(a.name()) << ", " << a.id() <<") nodes going from: " << QString::fromStdString(orig) << " to: " << QString::fromStdString(dest);
56+
qWarning() << __FUNCTION__ << ":"<<__LINE__<< " Cannot find " << QString::fromStdString(edge_type) << " edge between Parent (" << QString::fromStdString(p_node->name()) << ", " << p_node->id() <<") and son (" << QString::fromStdString(a.name()) << ", " << a.id() <<") nodes going from: " << QString::fromStdString(orig) << " to: " << QString::fromStdString(dest);
5557
return {};
5658
}
5759
if( auto rtmat = rt->get_edge_RT_as_rtmat(edge_rt.value(), timestamp); rtmat.has_value())
@@ -68,10 +70,10 @@ std::optional<Mat::RTMat> InnerEigenAPI::get_transformation_matrix(const std::st
6870
auto p_node = G->get_parent_node(b);
6971
if(not p_node.has_value())
7072
break;
71-
auto edge_rt = rt->get_edge_RT(p_node.value(), b.id());
73+
auto edge_rt = rt->get_edge_RT(p_node.value(), b.id(), edge_type);
7274
if (not edge_rt.has_value())
7375
{
74-
qWarning() << __FUNCTION__ << ":"<<__LINE__ << " Cannot find RT edge between Parent (" << QString::fromStdString(p_node->name()) << ", " << p_node->id() <<") and son (" << QString::fromStdString(a.name()) << ", " << a.id() <<") nodes going from: " << QString::fromStdString(orig) << " to: " << QString::fromStdString(dest);
76+
qWarning() << __FUNCTION__ << ":"<<__LINE__ << " Cannot find " << QString::fromStdString(edge_type) << " edge between Parent (" << QString::fromStdString(p_node->name()) << ", " << p_node->id() <<") and son (" << QString::fromStdString(a.name()) << ", " << a.id() <<") nodes going from: " << QString::fromStdString(orig) << " to: " << QString::fromStdString(dest);
7577
return {};
7678
}
7779
if( auto rtmat = rt->get_edge_RT_as_rtmat(edge_rt.value(), timestamp); rtmat.has_value())
@@ -91,16 +93,16 @@ std::optional<Mat::RTMat> InnerEigenAPI::get_transformation_matrix(const std::st
9193
if(p_node.has_value() and q_node.has_value())
9294
{
9395
//qDebug() << "listas A&B" << p_node.value().id() << q_node.value().id();
94-
auto a_edge_rt = rt->get_edge_RT(p_node.value(), a.id());
96+
auto a_edge_rt = rt->get_edge_RT(p_node.value(), a.id(), edge_type);
9597
if (not a_edge_rt.has_value())
9698
{
97-
qWarning() << __FUNCTION__ << ":"<<__LINE__ << " Cannot find RT edge between Parent (" << QString::fromStdString(p_node->name()) << ", " << p_node->id() <<") and son (" << QString::fromStdString(a.name()) << ", " << a.id() <<") nodes going from: " << QString::fromStdString(orig) << " to: " << QString::fromStdString(dest);
99+
qWarning() << __FUNCTION__ << ":"<<__LINE__ << " Cannot find " << QString::fromStdString(edge_type) << " edge between Parent (" << QString::fromStdString(p_node->name()) << ", " << p_node->id() <<") and son (" << QString::fromStdString(a.name()) << ", " << a.id() <<") nodes going from: " << QString::fromStdString(orig) << " to: " << QString::fromStdString(dest);
98100
return {};
99101
}
100-
auto b_edge_rt = rt->get_edge_RT(q_node.value(), b.id());
102+
auto b_edge_rt = rt->get_edge_RT(q_node.value(), b.id(), edge_type);
101103
if (not b_edge_rt.has_value())
102104
{
103-
qWarning() << __FUNCTION__ << ":"<<__LINE__ << " Cannot find RT edge between Parent (" << QString::fromStdString(p_node->name()) << ", " << p_node->id() <<") and son (" << QString::fromStdString(a.name()) << ", " << a.id() <<") nodes going from: " << QString::fromStdString(orig) << " to: " << QString::fromStdString(dest);
105+
qWarning() << __FUNCTION__ << ":"<<__LINE__ << " Cannot find " << QString::fromStdString(edge_type) << " edge between Parent (" << QString::fromStdString(p_node->name()) << ", " << p_node->id() <<") and son (" << QString::fromStdString(a.name()) << ", " << a.id() <<") nodes going from: " << QString::fromStdString(orig) << " to: " << QString::fromStdString(dest);
104106
return {};
105107
}
106108
auto a_rtmat = rt->get_edge_RT_as_rtmat(a_edge_rt.value(), timestamp);
@@ -133,9 +135,11 @@ std::optional<Mat::RTMat> InnerEigenAPI::get_transformation_matrix(const std::st
133135
}
134136
}
135137

136-
std::optional<Mat::Rot3D> InnerEigenAPI::get_rotation_matrix(const std::string &dest, const std::string &orig, std::uint64_t timestamp)
138+
std::optional<Mat::Rot3D> InnerEigenAPI::get_rotation_matrix(const std::string &dest, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type)
137139
{
138-
if( auto r = get_transformation_matrix(dest, orig, timestamp); r.has_value())
140+
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
141+
return {};
142+
if( auto r = get_transformation_matrix(dest, orig, timestamp, edge_type); r.has_value())
139143
return r.value().rotation();
140144
else
141145
{
@@ -144,9 +148,11 @@ std::optional<Mat::Rot3D> InnerEigenAPI::get_rotation_matrix(const std::string &
144148
}
145149
}
146150

147-
std::optional<Mat::Vector3d> InnerEigenAPI::get_translation_vector(const std::string &dest, const std::string &orig, std::uint64_t timestamp)
151+
std::optional<Mat::Vector3d> InnerEigenAPI::get_translation_vector(const std::string &dest, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type)
148152
{
149-
if (auto r = get_transformation_matrix(dest, orig, timestamp); r.has_value())
153+
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
154+
return {};
155+
if (auto r = get_transformation_matrix(dest, orig, timestamp, edge_type); r.has_value())
150156
return r.value().translation();
151157
else
152158
{
@@ -155,9 +161,11 @@ std::optional<Mat::Vector3d> InnerEigenAPI::get_translation_vector(const std::st
155161
return {};
156162
}
157163
}
158-
std::optional<Mat::Vector3d> InnerEigenAPI::get_euler_xyz_angles(const std::string &dest, const std::string &orig, std::uint64_t timestamp)
164+
std::optional<Mat::Vector3d> InnerEigenAPI::get_euler_xyz_angles(const std::string &dest, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type)
159165
{
160-
if( auto r = get_transformation_matrix(dest, orig, timestamp); r.has_value())
166+
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
167+
return {};
168+
if( auto r = get_transformation_matrix(dest, orig, timestamp, edge_type); r.has_value())
161169
return r.value().rotation().eulerAngles(0,1,2); //X Y Z order
162170
else
163171
{
@@ -169,10 +177,12 @@ std::optional<Mat::Vector3d> InnerEigenAPI::get_euler_xyz_angles(const std::stri
169177
////////////////////////////////////////////////////////////////////////////////////////
170178
////// TRANSFORM
171179
////////////////////////////////////////////////////////////////////////////////////////
172-
std::optional<Mat::Vector3d> InnerEigenAPI::transform(const std::string &dest, const Mat::Vector3d &vector, const std::string &orig, std::uint64_t timestamp)
180+
std::optional<Mat::Vector3d> InnerEigenAPI::transform(const std::string &dest, const Mat::Vector3d &vector, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type)
173181
{
182+
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
183+
return {};
174184
//std::cout <<__FUNCTION__ << " " << initVec << std::endl;
175-
auto tm = get_transformation_matrix(dest, orig, timestamp);
185+
auto tm = get_transformation_matrix(dest, orig, timestamp, edge_type);
176186
//std::cout << __FUNCTION__ << " " << tm.value().matrix().format(CleanFmt) << std::endl;
177187
if(tm.has_value())
178188
{
@@ -183,14 +193,18 @@ std::optional<Mat::Vector3d> InnerEigenAPI::transform(const std::string &dest, c
183193
return {};
184194
}
185195

186-
std::optional<Mat::Vector3d> InnerEigenAPI::transform( const std::string &dest, const std::string & orig, std::uint64_t timestamp)
196+
std::optional<Mat::Vector3d> InnerEigenAPI::transform( const std::string &dest, const std::string & orig, std::uint64_t timestamp, const std::string &edge_type)
187197
{
188-
return transform(dest, Mat::Vector3d(0.,0.,0.), orig, timestamp);
198+
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
199+
return {};
200+
return transform(dest, Mat::Vector3d(0.,0.,0.), orig, timestamp, edge_type);
189201
}
190202

191-
std::optional<Mat::Vector6d> InnerEigenAPI::transform_axis(const std::string &dest, const Mat::Vector6d &vector, const std::string &orig, std::uint64_t timestamp)
203+
std::optional<Mat::Vector6d> InnerEigenAPI::transform_axis(const std::string &dest, const Mat::Vector6d &vector, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type)
192204
{
193-
auto tm = get_transformation_matrix(dest, orig, timestamp);
205+
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
206+
return {};
207+
auto tm = get_transformation_matrix(dest, orig, timestamp, edge_type);
194208
if(tm.has_value())
195209
{
196210
const Mat::RTMat rtmat = tm.value();
@@ -208,9 +222,12 @@ std::optional<Mat::Vector6d> InnerEigenAPI::transform_axis(const std::string &de
208222
return {};
209223
}
210224

211-
std::optional<Mat::Vector6d> InnerEigenAPI::transform_axis( const std::string &dest, const std::string & orig, std::uint64_t timestamp)
225+
std::optional<Mat::Vector6d> InnerEigenAPI::transform_axis( const std::string &dest, const std::string & orig, std::uint64_t timestamp, const std::string &edge_type)
212226
{
213-
return transform_axis(dest, Mat::Vector6d::Zero(), orig, timestamp);
227+
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
228+
return {};
229+
Mat::Vector6d v;
230+
return transform_axis(dest, Mat::Vector6d::Zero(), orig, timestamp, edge_type);
214231
}
215232

216233
////////////////////////////////////////////////////////////////////////
@@ -356,4 +373,4 @@ void InnerEigenAPI::remove_cache_entry(uint64_t id)
356373
// cache[key] = ret;
357374
// }
358375
// return ret;
359-
//}
376+
//}

‎api/dsr_rt_api.cpp‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -9,23 +9,27 @@ RT_API::RT_API(DSR::DSRGraph *G_)
99
G = G_;
1010
}
1111

12-
std::optional<Edge> RT_API::get_edge_RT(const Node &n, uint64_t to)
12+
std::optional<Edge> RT_API::get_edge_RT(const Node &n, uint64_t to, const std::string &edge_type)
1313
{
14+
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
15+
return {};
1416
auto edges_ = n.fano();
15-
auto res = edges_.find({to, "RT"});
17+
auto res = edges_.find({to, edge_type});
1618
if (res != edges_.end())
1719
return res->second;
1820
else
1921
return {};
2022
}
2123

22-
std::optional<Mat::RTMat> RT_API::get_RT_pose_from_parent(const Node &n)
24+
std::optional<Mat::RTMat> RT_API::get_RT_pose_from_parent(const Node &n, const std::string &edge_type)
2325
{
26+
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
27+
return {};
2428
auto p = G->get_parent_node(n);
2529
if (p.has_value())
2630
{
2731
auto edges_ = p->fano();
28-
auto res = edges_.find({n.id(),"RT"});
32+
auto res = edges_.find({n.id(), edge_type});
2933
if (res != edges_.end())
3034
{
3135
auto r = G->get_attrib_by_name<rt_rotation_euler_xyz_att>(res->second);

‎api/include/dsr/api/dsr_api.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -563,6 +563,14 @@ namespace DSR
563563
return ret_vec;
564564
}
565565

566+
//////////////////////////////////////////////////
567+
///// Check RTs types
568+
/////////////////////////////////////////////////
569+
static constexpr std::array valid_edge_types = {"RT"sv, "VRT"sv};
570+
static constexpr bool is_valid_edge_type(std::string_view edge_type) {
571+
return std::ranges::find(valid_edge_types, edge_type) != valid_edge_types.end();
572+
}
573+
566574

567575
//////////////////////////////////////////////////
568576
///// QueuedSignals for python

‎api/include/dsr/api/dsr_inner_eigen_api.h‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@ namespace DSR
1717
class InnerEigenAPI : public QObject
1818
{
1919
Q_OBJECT
20-
using KeyTransform = std::tuple<std::string, std::string>;
20+
using KeyTransform = std::tuple<std::string, std::string, std::string>;
2121
using NodeReference = std::map<uint64_t , std::list<KeyTransform>>;
2222
using TransformCache = std::map<KeyTransform, Mat::RTMat>;
2323
using NodeMatrix = std::tuple<uint64_t , Mat::RTMat>;
@@ -28,18 +28,18 @@ namespace DSR
2828
/////////////////////////////////////////////////
2929
/// Kinematic transformation methods
3030
////////////////////////////////////////////////
31-
std::optional<Mat::Vector3d> transform( const std::string &dest, const std::string &orig, std::uint64_t timestamp = 0);
32-
std::optional<Mat::Vector3d> transform( const std::string &dest, const Mat::Vector3d &vector, const std::string &orig, std::uint64_t timestamp = 0);
33-
std::optional<Mat::Vector6d> transform_axis(const std::string &dest, const std::string & orig, std::uint64_t timestamp = 0);
34-
std::optional<Mat::Vector6d> transform_axis(const std::string &dest, const Mat::Vector6d &vector, const std::string &orig, std::uint64_t timestamp = 0);
31+
std::optional<Mat::Vector3d> transform( const std::string &dest, const std::string &orig, std::uint64_t timestamp = 0, const std::string &edge_type="RT");
32+
std::optional<Mat::Vector3d> transform( const std::string &dest, const Mat::Vector3d &vector, const std::string &orig, std::uint64_t timestamp = 0, const std::string &edge_type="RT");
33+
std::optional<Mat::Vector6d> transform_axis(const std::string &dest, const std::string & orig, std::uint64_t timestamp = 0, const std::string &edge_type="RT");
34+
std::optional<Mat::Vector6d> transform_axis(const std::string &dest, const Mat::Vector6d &vector, const std::string &orig, std::uint64_t timestamp = 0, const std::string &edge_type="RT");
3535

3636
////////////////////////////////////////////////
3737
/// Transformation matrix retrieval methods
3838
////////////////////////////////////////////////
39-
std::optional<Mat::RTMat> get_transformation_matrix(const std::string &dest, const std::string &orig, std::uint64_t timestamp = 0);
40-
std::optional<Mat::Rot3D> get_rotation_matrix(const std::string &dest, const std::string &orig, std::uint64_t timestamp = 0);
41-
std::optional<Mat::Vector3d> get_translation_vector(const std::string &dest, const std::string &orig, std::uint64_t timestamp = 0);
42-
std::optional<Mat::Vector3d> get_euler_xyz_angles(const std::string &dest, const std::string &orig, std::uint64_t timestamp = 0);
39+
std::optional<Mat::RTMat> get_transformation_matrix(const std::string &dest, const std::string &orig, std::uint64_t timestamp = 0, const std::string &edge_type="RT");
40+
std::optional<Mat::Rot3D> get_rotation_matrix(const std::string &dest, const std::string &orig, std::uint64_t timestamp = 0, const std::string &edge_type="RT");
41+
std::optional<Mat::Vector3d> get_translation_vector(const std::string &dest, const std::string &orig, std::uint64_t timestamp = 0, const std::string &edge_type="RT");
42+
std::optional<Mat::Vector3d> get_euler_xyz_angles(const std::string &dest, const std::string &orig, std::uint64_t timestamp = 0, const std::string &edge_type="RT");
4343

4444
public slots:
4545
void add_or_assign_edge_slot(uint64_t from, uint64_t to, const std::string& edge_type);

‎api/include/dsr/api/dsr_rt_api.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -24,8 +24,8 @@ namespace DSR
2424
void insert_or_assign_edge_RT(Node &n, uint64_t to, const std::vector<float> &trans, const std::vector<float> &rot_euler, std::optional<uint64_t> timestamp = std::nullopt);
2525
void insert_or_assign_edge_RT(Node &n, uint64_t to, std::vector<float> &&trans, std::vector<float> &&rot_euler, std::optional<uint64_t> timestamp = std::nullopt);
2626

27-
static std::optional<Edge> get_edge_RT(const Node &n, uint64_t to);
28-
std::optional<Mat::RTMat> get_RT_pose_from_parent(const Node &n);
27+
static std::optional<Edge> get_edge_RT(const Node &n, uint64_t to, const std::string &edge_type = "RT");
28+
std::optional<Mat::RTMat> get_RT_pose_from_parent(const Node &n, const std::string &edge_type = "RT");
2929
std::optional<Mat::RTMat> get_edge_RT_as_rtmat(const Edge &edge, std::uint64_t timestamp = 0);
3030
std::optional<Eigen::Vector3d> get_translation(const Node &n, uint64_t to, std::uint64_t timestamp = 0);
3131
std::optional<Eigen::Vector3d> get_translation(uint64_t node_id, uint64_t to, std::uint64_t timestamp = 0);

‎core/include/dsr/core/types/type_checking/dsr_attr_name.h‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -109,8 +109,12 @@ REGISTER_TYPE(rt_translation, std::reference_wrapper<const std::vector<float>>,
109109
REGISTER_TYPE(rt_quaternion, std::reference_wrapper<const std::vector<float>>, true)
110110
REGISTER_TYPE(rt_translation_velocity, std::reference_wrapper<const std::vector<float>>, true)
111111
REGISTER_TYPE(rt_rotation_euler_xyz_velocity, std::reference_wrapper<const std::vector<float>>, true)
112+
REGISTER_TYPE(rt_translation_acceleration, std::reference_wrapper<const std::vector<float>>, true)
113+
REGISTER_TYPE(rt_rotation_euler_xyz_acceleration, std::reference_wrapper<const std::vector<float>>, true)
112114
REGISTER_TYPE(rt_timestamps, std::reference_wrapper<const std::vector<uint64_t>> , false)
113115
REGISTER_TYPE(rt_se2_covariance, std::reference_wrapper<const std::vector<float>>, true)
116+
REGISTER_TYPE(rt_se2_covariance_velocity, std::reference_wrapper<const std::vector<float>>, true)
117+
REGISTER_TYPE(rt_se2_covariance_acceleration, std::reference_wrapper<const std::vector<float>>, true)
114118
REGISTER_TYPE(rt_head_index, int, false)
115119

116120

@@ -374,6 +378,15 @@ REGISTER_TYPE(imu_compass, float, false)
374378
REGISTER_TYPE(imu_time_stamp, float, false)
375379
REGISTER_TYPE(imu_sensor_tick, float, false)
376380

381+
REGISTER_TYPE(imu_linear_pose, std::reference_wrapper<const std::vector<float>>, false)
382+
REGISTER_TYPE(imu_angular_euler_xyz_pose, std::reference_wrapper<const std::vector<float>>, false)
383+
REGISTER_TYPE(imu_angular_quaternion_pose, std::reference_wrapper<const std::vector<float>>, false)
384+
REGISTER_TYPE(imu_linear_velocity, std::reference_wrapper<const std::vector<float>>, false)
385+
REGISTER_TYPE(imu_angular_velocity, std::reference_wrapper<const std::vector<float>>, false)
386+
REGISTER_TYPE(imu_linear_acceleration, std::reference_wrapper<const std::vector<float>>, false)
387+
REGISTER_TYPE(imu_angular_acceleration, std::reference_wrapper<const std::vector<float>>, false)
388+
389+
377390

378391
/*
379392
* SERVO

‎core/include/dsr/core/types/type_checking/dsr_edge_type.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ struct EdgeType {
3030
inline std::unordered_set<std::string_view> edge_types::set_type_;
3131

3232
REGISTER_EDGE_TYPE(RT)
33+
REGISTER_EDGE_TYPE(VRT)
3334
REGISTER_EDGE_TYPE(reachable)
3435
REGISTER_EDGE_TYPE(in)
3536
REGISTER_EDGE_TYPE(knows)

0 commit comments

Comments
 (0)