Eigen::Matrix4f Feature::getTransformFromMatches(bool &valid,
const vector_eigen_vector3f &earlier,
const vector_eigen_vector3f &now,
const InputVector &matches,
float max_dist /* = -1.0 */)
{
pcl::TransformationFromCorrespondences tfc;
valid = true;
vector<Eigen::Vector3f> t, f;
for (InputVector::const_iterator it = matches.begin() ; it != matches.end(); it++)
{
int this_id = it->queryIdx;
int earlier_id = it->trainIdx;
Eigen::Vector3f from(now[this_id][0],
now[this_id][1],
now[this_id][2]);
Eigen::Vector3f to(earlier[earlier_id][0],
earlier[earlier_id][1],
earlier[earlier_id][2]);
if (max_dist > 0)
{
// storing is only necessary, if max_dist is given
f.push_back(from);
t.push_back(to);
}
tfc.add(from, to, 1.0 /*/ to(2)*/); //the further, the less weight b/c of accuracy decay
}
// find smallest distance between a point and its neighbour in the same cloud
if (max_dist > 0)
{
//float min_neighbour_dist = 1e6;
Eigen::Matrix4f foo;
valid = true;
for (unsigned int i = 0; i < f.size(); i++)
{
float d_f = (f.at((i + 1) % f.size()) - f.at(i)).norm();
float d_t = (t.at((i + 1) % t.size()) - t.at(i)).norm();
if (abs(d_f - d_t) > max_dist)
{
valid = false;
return Eigen::Matrix4f();
}
}
}
// get relative movement from samples
return tfc.getTransformation().matrix();
}
void OperatorItem::initialize()
{
typedef std::vector<const stromx::runtime::Input*> InputVector;
typedef std::vector<const stromx::runtime::Output*> OutputVector;
InputVector inputs = m_model->op()->info().inputs();
qreal firstInputYPos = computeFirstYPos(inputs.size());
qreal yOffset = ConnectorItem::SIZE + CONNECTOR_OFFSET;
qreal inputXPos = -SIZE/2 - ConnectorItem::SIZE/2;
unsigned int i = 0;
for(InputVector::iterator iter = inputs.begin();
iter != inputs.end();
++iter, ++i)
{
ConnectorItem* inputItem = new ConnectorItem(this->m_model, (*iter)->id(), ConnectorItem::INPUT, this);
inputItem->setPos(inputXPos, firstInputYPos + i*yOffset);
m_inputs[(*iter)->id()] = inputItem;
}
OutputVector outputs = m_model->op()->info().outputs();
qreal firstOutputYPos = computeFirstYPos(outputs.size());
qreal outputXPos = SIZE/2 + ConnectorItem::SIZE/2;
i = 0;
for(OutputVector::iterator iter = outputs.begin();
iter != outputs.end();
++iter, ++i)
{
ConnectorItem* outputItem = new ConnectorItem(this->m_model, (*iter)->id(), ConnectorItem::OUTPUT, this);
outputItem->setPos(outputXPos, firstOutputYPos + i*yOffset);
m_outputs[(*iter)->id()] = outputItem;
}
}
