Eigen::Matrix4d recenter_transform3d(const Eigen::Matrix4d &transfo, const Eigen::Vector3d& center)
{
//% remove former translation part
Eigen::Matrix4d res = Eigen::Matrix4d::Identity();
res.block(0, 0, 3, 3) = transfo.block(0, 0, 3, 3);
//% create translations
Eigen::Matrix4d t1 = create_translation3d(-center);
Eigen::Matrix4d t2 = create_translation3d(center);
//% compute translated transform
res = t2*res*t1;
return res;
}
bool PairRelationDetector::has_trans_relation(int id1, int id2)
{
Eigen::Vector3d transVec = nodesCenter_[id1] - nodesCenter_[id2];
Eigen::Matrix4d transMat = create_translation3d(transVec);
Eigen::MatrixXd newverts2 = transform_point3d(nodesCpts_[id2], transMat);
double min_dist, mean_dist, max_dist;
distanceBetween(nodesCpts_[id1], newverts2, min_dist, mean_dist, max_dist);
double error = 2 * mean_dist / (nodesDiameter_[id1] + nodesDiameter_[id2]);
double nerror = fixDeviationByPartName(graph_->nodes[id1]->id, graph_->nodes[id2]->id, error);
if (nerror < thTransRadio_)
{
PairRelation pr(graph_->nodes[id1], graph_->nodes[id2]);
pr.trans_vec = transVec;
pr.diameter = computePairDiameter(pr);
pr.deviation = error;
pr.tag = true;
transPairs_.push_back(pr);
return true;
}
else
return false;
}
double PairRelationDetector::TransPairModifier::operator() (Structure::Node *n1, Eigen::MatrixXd& m1, Structure::Node *n2, Eigen::MatrixXd& m2)
{
double deviation(-1.0), min_dist, mean_dist, max_dist;
if ( n1->id == n2->id)
return deviation;
Eigen::Vector3d transVec = n1->center() - n2->center();
Eigen::Matrix4d transMat = create_translation3d(transVec);
Eigen::MatrixXd newverts2 = transform_point3d(m2, transMat);
distanceBetween(m1, newverts2, min_dist, mean_dist, max_dist);
deviation = 2 * mean_dist / (n1->bbox().diagonal().norm() + n2->bbox().diagonal().norm());
if ( deviation > maxAllowDeviation_)
{
maxAllowDeviation_ = deviation;
}
return deviation;
}
