C++ Transform3f::rotationVecVec方法代码示例

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;
//.........这里部分代码省略.........

//.........这里部分代码省略.........

         // 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