1 |
|
|
/* |
2 |
|
|
* Copyright 2011, Nicolas Mansard, LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#include <dynamic-graph/factory.h> |
7 |
|
|
|
8 |
|
|
#include <sot/core/debug.hh> |
9 |
|
|
#include <sot/core/visual-point-projecter.hh> |
10 |
|
|
|
11 |
|
|
namespace dynamicgraph { |
12 |
|
|
namespace sot { |
13 |
|
|
|
14 |
|
|
namespace dg = ::dynamicgraph; |
15 |
|
|
using namespace dg; |
16 |
|
|
|
17 |
|
|
/* --- DG FACTORY ------------------------------------------------------- */ |
18 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VisualPointProjecter, |
19 |
|
|
"VisualPointProjecter"); |
20 |
|
|
|
21 |
|
|
/* --- CONSTRUCTION ----------------------------------------------------- */ |
22 |
|
|
/* --- CONSTRUCTION ----------------------------------------------------- */ |
23 |
|
|
/* --- CONSTRUCTION ----------------------------------------------------- */ |
24 |
|
|
VisualPointProjecter::VisualPointProjecter(const std::string &name) |
25 |
|
|
: Entity(name) |
26 |
|
|
|
27 |
|
|
, |
28 |
|
|
CONSTRUCT_SIGNAL_IN(point3D, dynamicgraph::Vector), |
29 |
|
|
CONSTRUCT_SIGNAL_IN(transfo, MatrixHomogeneous) |
30 |
|
|
|
31 |
|
|
, |
32 |
|
|
CONSTRUCT_SIGNAL_OUT(point3Dgaze, dynamicgraph::Vector, |
33 |
|
|
m_point3DSIN << m_transfoSIN), |
34 |
|
|
CONSTRUCT_SIGNAL_OUT(depth, double, m_point3DgazeSOUT), |
35 |
|
|
CONSTRUCT_SIGNAL_OUT(point2D, dynamicgraph::Vector, |
36 |
|
|
m_point3DgazeSOUT << m_depthSOUT) { |
37 |
|
|
Entity::signalRegistration(m_point3DSIN); |
38 |
|
|
Entity::signalRegistration(m_transfoSIN); |
39 |
|
|
Entity::signalRegistration(m_point3DgazeSOUT); |
40 |
|
|
Entity::signalRegistration(m_point2DSOUT); |
41 |
|
|
Entity::signalRegistration(m_depthSOUT); |
42 |
|
|
} |
43 |
|
|
|
44 |
|
|
/* --- SIGNALS ---------------------------------------------------------- */ |
45 |
|
|
/* --- SIGNALS ---------------------------------------------------------- */ |
46 |
|
|
/* --- SIGNALS ---------------------------------------------------------- */ |
47 |
|
|
|
48 |
|
|
dynamicgraph::Vector &VisualPointProjecter::point3DgazeSOUT_function( |
49 |
|
|
dynamicgraph::Vector &p3g, int iter) { |
50 |
|
|
const dynamicgraph::Vector &p3 = m_point3DSIN(iter); |
51 |
|
|
const MatrixHomogeneous &M = m_transfoSIN(iter); |
52 |
|
|
MatrixHomogeneous Mi; |
53 |
|
|
Mi = M.inverse(Eigen::Affine); |
54 |
|
|
p3g = Mi.matrix() * p3; |
55 |
|
|
return p3g; |
56 |
|
|
} |
57 |
|
|
|
58 |
|
|
dynamicgraph::Vector &VisualPointProjecter::point2DSOUT_function( |
59 |
|
|
dynamicgraph::Vector &p2, int iter) { |
60 |
|
|
sotDEBUGIN(15); |
61 |
|
|
|
62 |
|
|
const dynamicgraph::Vector &p3 = m_point3DgazeSOUT(iter); |
63 |
|
|
const double &z = m_depthSOUT(iter); |
64 |
|
|
assert(z > 0); |
65 |
|
|
|
66 |
|
|
p2.resize(2); |
67 |
|
|
p2(0) = p3(0) / z; |
68 |
|
|
p2(1) = p3(1) / z; |
69 |
|
|
|
70 |
|
|
sotDEBUGOUT(15); |
71 |
|
|
return p2; |
72 |
|
|
} |
73 |
|
|
|
74 |
|
|
double &VisualPointProjecter::depthSOUT_function(double &z, int iter) { |
75 |
|
|
const dynamicgraph::Vector &p3 = m_point3DgazeSOUT(iter); |
76 |
|
|
assert(p3.size() == 3); |
77 |
|
|
z = p3(2); |
78 |
|
|
return z; |
79 |
|
|
} |
80 |
|
|
|
81 |
|
|
/* --- ENTITY ----------------------------------------------------------- */ |
82 |
|
|
/* --- ENTITY ----------------------------------------------------------- */ |
83 |
|
|
/* --- ENTITY ----------------------------------------------------------- */ |
84 |
|
|
|
85 |
|
|
void VisualPointProjecter::display(std::ostream &os) const { |
86 |
|
|
os << "VisualPointProjecter " << getName(); |
87 |
|
|
} |
88 |
|
|
|
89 |
|
|
} // namespace sot |
90 |
|
|
} // namespace dynamicgraph |