PairRelationDetector.cpp
#include "PairRelationDetector.h"
#pragma warning(disable:4100 4101)
void PairRelationDetector::pushToOtherPairs(QVector<PairRelation>& prs, QString type)
{
for (QVector<PairRelation>::iterator it = prs.begin(); it != prs.end(); ++it)
{
if ( it->tag)
{
it->type = type;
otherPairs_.push_back(*it);
}
}
}
double PairRelationDetector::ConnectedPairModifier::operator() (Structure::Node *n1, Eigen::MatrixXd& m1, Structure::Node *n2, Eigen::MatrixXd& m2)
{
double deviation(-1.0),mean_dist, max_dist;
if ( n1->id == n2->id)
return deviation;
Structure::Link* link = RelationDetector::findLink(n1, n2, this->graph_);
if ( link == 0)
return deviation;
if ( this->bUseLink_)
deviation = RelationDetector::computeDeviationByLink(link);
else
{
distanceBetween(m1, m2, deviation, mean_dist, max_dist);
}
deviation = deviation/this->normalizeCoef_;
if ( deviation > this->maxAllowDeviation_)
{
this->maxAllowDeviation_ = deviation;
}
return deviation;
}
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;
}
double PairRelationDetector::RefPairModifier::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 refCenter = (n1->center() + n2->center())*0.5;
Eigen::Vector3d refNormal = n1->center() - n2->center();
refNormal.normalize();
Eigen::MatrixXd newverts2;
reflect_points3d(m2, refCenter, refNormal, newverts2);
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;
}
//////////////////////////////////////////////////////
PairRelationDetector::PairRelationDetector(Structure::Graph* g, int ith, double normalizeCoef, bool bUseLink, bool bModifyDeviation, int logLevel)
:RelationDetector(g, "PairRelationDetector-", ith, normalizeCoef, 1, logLevel)
{
bSource_ = ith == 0;
bUseLink_ = bUseLink;
bModifyDeviation_ = bModifyDeviation;
if ( this->logLevel_ > 0)
{
logStream_ << "Point level: " << pointLevel_ << "\n";
logStream_ << "Radius Radio: " << thRadiusRadio_ << "\n";
logStream_ << "Trans Radio: " << thTransRadio_ << "\n";
logStream_ << "Ref Radio: " << thRefRadio_ << "\n";
logStream_ << "Axis deviation Radio: " << thAxisDeviationRadio_ << "\n";
if ( this->bUseLink_)
{
logStream_ << "Connected pairs use link distance \n";
}
else
{
logStream_ << "Connected pairs use skeleton distance \n";
}
}
for ( int i = 0; i < (int) graph_->nodes.size(); ++i)
{
Structure::Node * n = graph_->nodes[i];
std::vector<Eigen::Vector3d> nodeCptsV;
extractCpts( n, nodeCptsV, pointLevel_);
Eigen::MatrixXd nodeCptsM;
vectorPts2MatrixPts(nodeCptsV, nodeCptsM);
nodesCpts_.push_back(nodeCptsM);
nodesCenter_.push_back( Eigen::Vector3d(nodeCptsM.col(0).mean(), nodeCptsM.col(1).mean(), nodeCptsM.col(2).mean()) );
nodesDiameter_.push_back(n->bbox().diagonal().norm());
}
}
Eigen::MatrixXd& PairRelationDetector::findCptsByNodeId(Structure::Node* node)
{
for ( int i = 0; i < (int) graph_->nodes.size(); ++i)
{
Structure::Node * n = graph_->nodes[i];
if (node == n)
{
return nodesCpts_[i];
}
}
return nodesCpts_[0];
}
void PairRelationDetector::detectConnectedPairs(Structure::Graph* g, QVector<PART_LANDMARK> &corres)
{
QVector<PairRelation> pairs;
double deviation, mean_dist, max_dist;;
int tmp1 = graph_->edges.size();
for ( int i = 0; i < tmp1; ++i)
{
Structure::Link *link = graph_->edges[i];
if ( this->bUseLink_)
deviation = RelationDetector::computeDeviationByLink(link);
else
{
Eigen::MatrixXd& ptsi = findCptsByNodeId(link->n1);
Eigen::MatrixXd& ptsj = findCptsByNodeId(link->n2);
distanceBetween(ptsi, ptsj, deviation, mean_dist, max_dist);
}
PairRelation prb(link->n1,link->n2);
prb.deviation = deviation/this->normalizeCoef_;
pairs.push_back(prb);
}
if (bModifyDeviation_)
{
if ( this->logLevel_ > 0)
{
logStream_ << "\nConnected pairs, modify deviation, normalize coeff is: " << this->normalizeCoef_ << "\n";
}
modifyPairsDegree(pairs, ConnectedPairModifier(g, normalizeCoef_, bUseLink_, pointLevel_), g, corres, "Connected pairs");
}
else
{
if ( this->logLevel_ > 0)
{
logStream_ << "\nConnected pairs, not modify deviation, normalize coeff is: " << this->normalizeCoef_ << "\n";
int i(0),j(0); double tmp(0.0);
for ( QVector<PairRelation>::iterator it = pairs.begin(); it != pairs.end(); ++it, ++i)
{
logStream_ << i << " " << *it << "\n";
if (it->deviation > tmp)
{
tmp = it->deviation;
j = i;
}
}
this->logStream_ << "pair " << j << " with max deviation: " << tmp << "\n\n";
}
}
connectedPairs_.clear();
for (QVector<PairRelation>::iterator it = pairs.begin(); it != pairs.end(); ++it)
{
if (it->tag)
{
it->type = CONNECTED;
connectedPairs_.push_back(*it);
}
}
if( logLevel_ >0 )
{
logStream_ << connectedPairs_.size() << " pairs used in tracing!" << "\n\n\n";
}
}
void PairRelationDetector::detectOtherPairs(Structure::Graph* g, QVector<PART_LANDMARK> &corres)
{
transPairs_.clear(); refPairs_.clear();
otherPairs_.clear();
int nNodes = graph_->nodes.size();
for (int i=0; i<nNodes; ++i)
{
Structure::Node *n1 = graph_->nodes[i];
double diam1 = n1->bbox().diagonal().norm();
for (int j=i+1; j<nNodes; ++j)
{
Structure::Node *n2 = graph_->nodes[j];
double diam2 = n2->bbox().diagonal().norm();
bool hasTrans(false),hasRef(false);
// compare radius // whether 2 part could be symmetry coarsely
if ( n1->type() == n2->type() )
{
if ( (diam1/diam2) < thRadiusRadio_ && (diam2/diam1) < thRadiusRadio_ )
{
hasTrans = has_trans_relation(i,j);//
if ( !hasTrans)
hasRef = has_ref_relation(i, j);
}
}
}
}
//
otherPairs_.clear();
//if (bModifyDeviation_)
//{
//modifyPairsDegree(transPairs_, TransPairModifier(pointLevel_), g, corres, "Trans pairs");
//modifyPairsDegree(refPairs_, RefPairModifier(pointLevel_), g, corres, "Reflection pairs");
//}
pushToOtherPairs(transPairs_, TRANS);
pushToOtherPairs(refPairs_, REF);
}
void PairRelationDetector::detect(Structure::Graph* g, QVector<PART_LANDMARK> &corres)
{
detectConnectedPairs(g, corres);
detectOtherPairs(g,corres);
}
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;
}
bool PairRelationDetector::has_ref_relation(int id1, int id2)
{
Eigen::Vector3d refCenter = (nodesCenter_[id1] + nodesCenter_[id2])*0.5;
Eigen::Vector3d refNormal = nodesCenter_[id1] - nodesCenter_[id2];
refNormal.normalize();
Eigen::MatrixXd newverts2;
reflect_points3d(nodesCpts_[id2], refCenter, refNormal, newverts2);
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( logLevel_ >1 )
{
logStream_ <<"<(" << graph_->nodes[id1]->id <<", " << graph_->nodes[id2]->id <<"), ref)> center: " << refCenter[0] << ", " << refCenter[1] <<", "<< refCenter[2] << "\n"
<<"normal: " << refNormal[0] <<", " << refNormal[1] << ", " << refNormal[2] << "\n";
logStream_ << "deviation : " << error << "\n";
logStream_ << "\n";
}
if ( nerror < thRefRadio_)
{
PairRelation pr(graph_->nodes[id1], graph_->nodes[id2]);
pr.diameter = computePairDiameter(pr);
pr.deviation = error;
pr.tag = true;
refPairs_.push_back(pr);
return true;
}
else
{
return false;
}
}
//bool PairRelationDetector::has_rot_relation(int id1, int id2)
//{
// Structure::Node * n1 = graph_->nodes[id1];
// Structure::Node * n2 = graph_->nodes[id2];
// if ( Structure::SHEET == n1->type() || Structure::SHEET == n2->type()) // jjcao todo, check planes have rot relation
// return false;
// Line_3 l1 = curve2line(n1);
// Line_3 l2 = curve2line(n2);
// Object result = intersection(l1, l2);
// if (const Point_3<Kernel> *ipoint = object_cast<Point_3<Kernel> >(&result))
// {
// Eigen::Vector3d center(ipoint->x(), ipoint->y(), ipoint->z());
//
// Vector_3 v1 = l1;
// Vector_3 v2 = l2;
// Eigen::Vector3d vec1(v1.x(), v1.y(), v1.z());
// Eigen::Vector3d vec2(v2.x(), v2.y(), v2.z());
// vec1.normalize(); vec2.normalize();
// Eigen::Vector3d direction = vec1.cross(vec2);
// //////////////////////////
// double angle = asin( vec1.cross(vec2).norm() );
// double error;
// Structure::Node * n;
// if ( vec1.dot(vec2) > 0)
// {
// n = n2->clone();
// rotNodeCpts(n, center, direction, angle);
// error = distanceBetweenTransNodes(n1, n);
// }
// else
// {
// n = n1->clone();
// rotNodeCpts(n, center, direction, angle);
// error = distanceBetweenTransNodes(n2, n);
// }
//
// //////////////////////////////
// if (error < thRot_)
// {
// PairRelation pairRel;
// pairRel.type = ROT;
// if ( vec1.dot(vec2) > 0)
// {
// pairRel.id1 = n1->id;
// pairRel.id2 = n2->id;
// }
// else
// {
// pairRel.id1 = n2->id;
// pairRel.id2 = n1->id;
// }
///* pairRel.center = center;
// pairRel.direction = direction;*/
// pairRelations_.push_back(pairRel);
// return true;
// }
//
// }
// else
// {
// return false;
// }
//}
