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; }