示例#1
0
bool Transform3f::bestFit (int npoints, const Vec3f points[], const Vec3f goals[], float *sqsum_out)
{
   QS_DEF(Array<double>, X); //set of points
   QS_DEF(Array<double>, Y); //set of goals
   Matr3x3d R, RT, RTR, evectors_matrix;
   //
   Matr3x3d rotation;
   double scale;
   Vec3f translation;
   //

   bool res = 1;
   Vec3f vec, tmp;
   double cpoints[3] = {0.0}, cgoals[3] = {0.0}; // centroid of points, of goals
   int i, j, k;
   
   for (i = 0; i < npoints; i++)
   {
      cpoints[0] += points[i].x;
      cpoints[1] += points[i].y;
      cpoints[2] += points[i].z;
      cgoals[0] += goals[i].x;
      cgoals[1] += goals[i].y;
      cgoals[2] += goals[i].z;
   }
   for (i = 0; i < 3; i++)
   {
      cpoints[i] /= npoints; 
      cgoals[i] /= npoints;
   }
   X.resize(npoints * 3);
   Y.resize(npoints * 3);

   //move each set to origin
   for (i = 0; i < npoints; i++)
   {
      X[i * 3 + 0] = points[i].x - cpoints[0];
      X[i * 3 + 1] = points[i].y - cpoints[1];
      X[i * 3 + 2] = points[i].z - cpoints[2];
      Y[i * 3 + 0] = goals[i].x - cgoals[0];
      Y[i * 3 + 1] = goals[i].y - cgoals[1];
      Y[i * 3 + 2] = goals[i].z - cgoals[2];
   }

   if (npoints > 1)
   {
      /* compute R */
      for (i = 0; i < 3; i++) 
      {
         for (j = 0; j < 3; j++) 
         {
            R.elements[i * 3 + j] = 0.0;
            for (k = 0; k < npoints; k++)
            {
               R.elements[i * 3 + j] += Y[k * 3 + i] * X[k * 3 + j];
            }
         }
      }
      
      //Compute R^T * R

      R.getTransposed(RT);
      RT.matrixMatrixMultiply(R, RTR);

      RTR.eigenSystem(evectors_matrix);

      if (RTR.elements[0] > 2 * EPSILON)
      {
         float norm_b0,norm_b1,norm_b2;
         Vec3f a0, a1, a2;
         Vec3f b0, b1, b2;
         
         a0.set((float)evectors_matrix.elements[0], (float)evectors_matrix.elements[3], (float)evectors_matrix.elements[6]);
         a1.set((float)evectors_matrix.elements[1], (float)evectors_matrix.elements[4], (float)evectors_matrix.elements[7]);
         a2.cross(a0, a1);

         R.matrixVectorMultiply(a0, b0);
         R.matrixVectorMultiply(a1, b1);
         norm_b0 = b0.length();
         norm_b1 = b1.length();
         Line3f l1, l2;
         float sqs1, sqs2;
         l1.bestFit(npoints, points, &sqs1);
         l2.bestFit(npoints, goals, &sqs2);
         if( sqs1 < 2 * EPSILON && sqs2 < 2 * EPSILON)
         {
            Transform3f temp;
            temp.rotationVecVec(l1.dir, l2.dir);
            for (i = 0; i < 3; i++)
               for (j = 0; j < 3; j++)
                  rotation.elements[i * 3 + j] = temp.elements[j * 4 + i];
         }
         else
         {
            b0.normalize();
            b1.normalize();
            b2.cross(b0, b1);
            norm_b2 = b2.length();
            
            evectors_matrix.elements[2] = a2.x;
            evectors_matrix.elements[5] = a2.y;
            evectors_matrix.elements[8] = a2.z;
            evectors_matrix.transpose();
                        
            RTR.elements[0] = b0.x; RTR.elements[1] = b1.x; RTR.elements[2] = b2.x;
            RTR.elements[3] = b0.y; RTR.elements[4] = b1.y; RTR.elements[5] = b2.y;
            RTR.elements[6] = b0.z; RTR.elements[7] = b1.z; RTR.elements[8] = b2.z;
            RTR.matrixMatrixMultiply(evectors_matrix, rotation);
         }
      }
      else
      {
         res = 0;
      }
   }
   else
   {
      res = 0;
   }
   if (!res)
   {
      rotation.identity();
   }
 
   //Calc scale
   scale = 1.0;
   if (res && npoints > 1) 
   {
      float l1 = 0.0;
      float l2 = 0.0;
      Vec3f vx, vy; 
      for (i = 0; i < npoints; i++) 
      {
         Vec3f vx((float)X[i * 3 + 0], (float)X[i * 3 + 1], (float)X[i * 3 + 2]);
         Vec3f vy((float)Y[i * 3 + 0], (float)Y[i * 3 + 1], (float)Y[i * 3 + 2]);
         rotation.matrixVectorMultiply(vx, vec);
         l1 += Vec3f::dot(vy, vec);
         l2 += Vec3f::dot(vec, vec);
      }
      scale = l1 / l2;
   }

   X.clear();
   Y.clear();

   //Calc translation
   translation.set((float)cgoals[0], (float)cgoals[1], (float)cgoals[2]);
   tmp = Vec3f((float)cpoints[0], (float)cpoints[1], (float)cpoints[2]);
   rotation.matrixVectorMultiply(tmp, vec);
   vec.scale((float)scale);
   translation.sub(vec);
   
   identity();
   for (i = 0; i < 3; i++)
   {
      for (j = 0; j < 3; j++)
      {
         elements[i * 4 + j] = (float)rotation.elements[j * 3 + i];
      }
   }
   elements[15] = 1.0f;
   translate(translation);
   for (i = 0; i < 3; i++)
   {
      for (j = 0; j < 3; j++)
      {
         elements[i * 4 + j] *= (float)scale;
      }
   }

   //Deviation
   if (sqsum_out)
   {
      *sqsum_out = 0;
      float d = .0f;
      for (i = 0; i < npoints; i++)
      {
         vec.pointTransformation(points[i], *this);
         d = Vec3f::dist(vec, goals[i]);
         *sqsum_out += d * d;
      }
   }
   return true;
}
示例#2
0
bool EdgeRotationMatcher::match (float rms_threshold, float eps)
{
   if (cb_get_xyz == 0)
      throw Error("cb_get_xyz not specified");

   if (_subgraph.vertexCount() < 2 || _subgraph.edgeCount() < 1)
      return true;

   QS_DEF(Array<int>, in_cycle);
   QS_DEF(Array<_DirEdge>, edge_queue);
   QS_DEF(Array<int>, vertex_queue);
   QS_DEF(Array<int>, states);

   in_cycle.clear_resize(_subgraph.edgeEnd());
   edge_queue.clear();
   states.clear_resize(__max(_subgraph.edgeEnd(), _subgraph.vertexEnd() + 1));
   
   int i, j, k, bottom;

   // Find all subgraph bridges
   SpanningTree spt(_subgraph, 0);

   in_cycle.zerofill();

   spt.markAllEdgesInCycles(in_cycle.ptr(), 1);

   // Find the first bridge, put it to the queue 2 times
   for (i = _subgraph.edgeBegin(); i < _subgraph.edgeEnd(); i = _subgraph.edgeNext(i))
      if (!in_cycle[i] && (cb_can_rotate == 0 || cb_can_rotate(_subgraph, i)))
      {
         const Edge &edge = _subgraph.getEdge(i);

         if (_mapping[edge.beg] < 0 || _mapping[edge.end] < 0)
            continue;

         edge_queue.push();
         edge_queue.top().idx = i;
         edge_queue.top().beg = edge.beg;
         edge_queue.top().end = edge.end;

         edge_queue.push();
         edge_queue.top().idx = i;
         edge_queue.top().beg = edge.end;
         edge_queue.top().end = edge.beg;
         break;
      }

   // If the queue is empty, then we have no bridge
   if (edge_queue.size() == 0)
   {
      GraphAffineMatcher afm(_subgraph, _supergraph, _mapping);

      afm.cb_get_xyz = cb_get_xyz;
      return afm.match(rms_threshold);
   }

   float scale = 1.f;

   // detect scaling factor by average bond length
   if (equalize_edges)
   {
      float sum_sub = 0.f, sum_super = 0.f;
      
      for (i = _subgraph.edgeBegin(); i < _subgraph.edgeEnd(); i = _subgraph.edgeNext(i))
      {
         const Edge &edge = _subgraph.getEdge(i);
         Vec3f beg, end;

         cb_get_xyz(_subgraph, edge.beg, beg);
         cb_get_xyz(_subgraph, edge.end, end);

         sum_sub += Vec3f::dist(beg, end);
      }

      for (i = _supergraph.edgeBegin(); i < _supergraph.edgeEnd(); i = _supergraph.edgeNext(i))
      {
         const Edge &edge = _supergraph.getEdge(i);
         Vec3f beg, end;

         cb_get_xyz(_supergraph, edge.beg, beg);
         cb_get_xyz(_supergraph, edge.end, end);

         sum_super += Vec3f::dist(beg, end);
      }

      if (sum_sub > EPSILON && sum_super > EPSILON)
      {
         sum_sub /= _subgraph.edgeCount();
         sum_super /= _supergraph.edgeCount();
         scale = sum_super / sum_sub;
      }
   }

   // save vertex positions

   QS_DEF(Array<Vec3f>, xyz_sub);
   QS_DEF(Array<Vec3f>, xyz_super);
   QS_DEF(Array<int>, xyzmap);

   xyzmap.clear_resize(_supergraph.vertexEnd());
   xyz_sub.clear();
   xyz_super.clear();

   for (i = _subgraph.vertexBegin(); i != _subgraph.vertexEnd();
        i = _subgraph.vertexNext(i))
   {
      if (_mapping[i] < 0)
         continue;

      Vec3f &pos_sub   = xyz_sub.push();
      Vec3f &pos_super = xyz_super.push();

      cb_get_xyz(_subgraph, i, pos_sub);
      cb_get_xyz(_supergraph, _mapping[i], pos_super);

      pos_sub.scale(scale);

      xyzmap[_mapping[i]] = xyz_sub.size() - 1;
   }

   // Make queue of edges
   states.zerofill();
   bottom = 0;

   while (edge_queue.size() != bottom)
   {
      // extract edge from queue
      int edge_end = edge_queue[bottom].end;
      int edge_idx = edge_queue[bottom].idx;
      bottom++;
      
      // mark it as 'completed'
      states[edge_idx] = 2;

      // look for neighbors
      const Vertex &end_vertex = _subgraph.getVertex(edge_end);

      for (i = end_vertex.neiBegin(); i != end_vertex.neiEnd(); i = end_vertex.neiNext(i))
      {
         int nei_edge_idx = end_vertex.neiEdge(i);

         // check that neighbor have 'untouched' status
         if (states[nei_edge_idx] != 0)
            continue;

         const Edge &nei_edge = _subgraph.getEdge(nei_edge_idx);
         int other_end = nei_edge.findOtherEnd(edge_end);

         if (_mapping[other_end] < 0)
            continue;

         // set status 'in process'
         states[nei_edge_idx] = 1;

         // push the neighbor edge to the queue
         edge_queue.push();
         edge_queue.top().idx = nei_edge_idx;
         edge_queue.top().beg = edge_end;
         edge_queue.top().end = other_end;
      }
   }

   // do initial transform (impose first subgraph edge in the queue on corresponding one in the graph)
   int beg2 = edge_queue[0].beg;
   int end2 = edge_queue[0].end;
   int beg1 = _mapping[beg2];
   int end1 = _mapping[end2];
   Vec3f g1_v1, g1_v2, g2_v1, g2_v2, diff1, diff2;
   Transform3f matr;

   cb_get_xyz(_supergraph, beg1, g1_v1);
   cb_get_xyz(_supergraph, end1, g1_v2);

   cb_get_xyz(_subgraph, beg2, g2_v1);
   cb_get_xyz(_subgraph, end2, g2_v2);

   g2_v1.scale(scale);
   g2_v2.scale(scale);

   diff1.diff(g1_v2, g1_v1);
   diff2.diff(g2_v2, g2_v1);

   matr.identity();
   if (!matr.rotationVecVec(diff2, diff1))
      throw Error("error calling RotationVecVec()");

   matr.translateLocal(-g2_v1.x, -g2_v1.y, -g2_v1.z);
   matr.translate(g1_v1);

   for (k = 0; k < xyz_sub.size(); k++)
      xyz_sub[k].transformPoint(matr);

   // for all edges in queue that are subject to rotate...
   for (i = 0; i < edge_queue.size(); i++)
   {
      int edge_beg = edge_queue[i].beg;
      int edge_end = edge_queue[i].end;
      int edge_idx = edge_queue[i].idx;

      if (in_cycle[edge_idx])
         continue;

      if (cb_can_rotate != 0 && !cb_can_rotate(_subgraph, edge_idx))
         continue;

      // start BFS from the end of the edge
      states.zerofill();
      states[edge_end] = 1;

      vertex_queue.clear();
      vertex_queue.push(edge_end);
      bottom = 0;

      while (vertex_queue.size() != bottom)
      {
         // extract vertex from queue
         const Vertex &vertex = _subgraph.getVertex(vertex_queue[bottom]);

         states[vertex_queue[bottom]] = 2;
         bottom++;

         // look over neighbors
         for (int j = vertex.neiBegin(); j != vertex.neiEnd(); j = vertex.neiNext(j))
         {
            int nei_idx = vertex.neiVertex(j);

            if (nei_idx == edge_beg)
               continue;
            if (states[nei_idx] != 0)
               continue;
            
            states[nei_idx] = 1;
            vertex_queue.push(nei_idx);
         }
      }

      // now states[j] == 0 if j-th vertex shound not be moved

      Vec3f edge_beg_pos, edge_end_pos, rot_axis;

      // get rotation axis
      edge_beg_pos.copy(xyz_sub[xyzmap[_mapping[edge_beg]]]);
      edge_end_pos.copy(xyz_sub[xyzmap[_mapping[edge_end]]]);

      rot_axis.diff(edge_end_pos, edge_beg_pos);
      if (!rot_axis.normalize())
         continue;

      const Vertex &edge_end_vertex = _subgraph.getVertex(edge_end);

      float max_sum_len = -1;

      for (j = edge_end_vertex.neiBegin(); j != edge_end_vertex.neiEnd();
           j = edge_end_vertex.neiNext(j))
      {
         int nei_idx_2 = edge_end_vertex.neiVertex(j);
         int nei_idx_1 = _mapping[nei_idx_2];

         if (nei_idx_2 == edge_beg)
            continue;

         if (nei_idx_1 == -1)
            continue;

         Vec3f nei1_pos;
         Vec3f nei2_pos;

         nei1_pos.copy(xyz_super[xyzmap[nei_idx_1]]);
         nei2_pos.copy(xyz_sub[xyzmap[_mapping[nei_idx_2]]]);

         nei1_pos.sub(edge_end_pos);
         nei2_pos.sub(edge_end_pos);

         float dot1 = Vec3f::dot(nei1_pos, rot_axis);
         float dot2 = Vec3f::dot(nei2_pos, rot_axis);

         nei1_pos.addScaled(rot_axis, -dot1);
         nei2_pos.addScaled(rot_axis, -dot2);

         if (max_sum_len > nei1_pos.length() + nei1_pos.length())
            continue;

         max_sum_len = nei1_pos.length() + nei1_pos.length();

         if (!nei1_pos.normalize() || !nei2_pos.normalize())
            continue;

         double dp = Vec3f::dot(nei1_pos, nei2_pos);

         if (dp > 1 - EPSILON)
            dp = 1 - EPSILON;
         if (dp < -1 + EPSILON)
            dp = -1 + EPSILON;

         double ang = acos(dp);

         Vec3f cross;

         cross.cross(nei1_pos, nei2_pos);

         if (Vec3f::dot(cross, rot_axis) < 0)
            ang = -ang;

         matr.rotation(rot_axis.x, rot_axis.y, rot_axis.z, (float)ang);
         matr.translateLocalInv(edge_end_pos);
         matr.translate(edge_end_pos);
      }

      if (max_sum_len > 0)
      {
         for (j = _subgraph.vertexBegin(); j < _subgraph.vertexEnd(); j = _subgraph.vertexNext(j))
            if (_mapping[j] >= 0 && states[j] != 0)
               xyz_sub[xyzmap[_mapping[j]]].transformPoint(matr);
      }
   }

   float sqsum = 0;

   for (k = 0; k < xyz_sub.size(); k++)
      sqsum += Vec3f::distSqr(xyz_sub[k], xyz_super[k]);

   sqsum = sqrt(sqsum / xyz_sub.size());

   if (sqsum > rms_threshold + eps)
      return false;

   return true;
}