コード例 #1
0
ファイル: Feature.cpp プロジェクト: weeks-yu/WReg
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();
}
コード例 #2
0
ファイル: OperatorItem.cpp プロジェクト: uboot/stromx-studio
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;
    }
}