Ejemplo n.º 1
0
 inline void resize(Eigen::MatrixXd & m,
                    vcl_size_t new_rows,
                    vcl_size_t new_cols)
 {
   m.resize(new_rows, new_cols);
 }
Ejemplo n.º 2
0
IGL_INLINE bool igl::copyleft::cgal::signed_distance_isosurface(
  const Eigen::MatrixXd & IV,
  const Eigen::MatrixXi & IF,
  const double level,
  const double angle_bound,
  const double radius_bound,
  const double distance_bound,
  const SignedDistanceType sign_type,
  Eigen::MatrixXd & V,
  Eigen::MatrixXi & F)
{
  using namespace std;
  using namespace Eigen;

  // default triangulation for Surface_mesher
  typedef CGAL::Surface_mesh_default_triangulation_3 Tr;
  // c2t3
  typedef CGAL::Complex_2_in_triangulation_3<Tr> C2t3;
  typedef Tr::Geom_traits GT;//Kernel
  typedef GT::Sphere_3 Sphere_3;
  typedef GT::Point_3 Point_3;
  typedef GT::FT FT;
  typedef std::function<FT (Point_3)> Function;
  typedef CGAL::Implicit_surface_3<GT, Function> Surface_3;

  AABB<Eigen::MatrixXd,3> tree;
  tree.init(IV,IF);

  Eigen::MatrixXd FN,VN,EN;
  Eigen::MatrixXi E;
  Eigen::VectorXi EMAP;
  WindingNumberAABB< Eigen::Vector3d, Eigen::MatrixXd, Eigen::MatrixXi > hier;
  switch(sign_type)
  {
    default:
      assert(false && "Unknown SignedDistanceType");
    case SIGNED_DISTANCE_TYPE_UNSIGNED:
      // do nothing
      break;
    case SIGNED_DISTANCE_TYPE_DEFAULT:
    case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
      hier.set_mesh(IV,IF);
      hier.grow();
      break;
    case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
      // "Signed Distance Computation Using the Angle Weighted Pseudonormal"
      // [Bærentzen & Aanæs 2005]
      per_face_normals(IV,IF,FN);
      per_vertex_normals(IV,IF,PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE,FN,VN);
      per_edge_normals(
        IV,IF,PER_EDGE_NORMALS_WEIGHTING_TYPE_UNIFORM,FN,EN,E,EMAP);
      break;
  }

  Tr tr;            // 3D-Delaunay triangulation
  C2t3 c2t3 (tr);   // 2D-complex in 3D-Delaunay triangulation
  // defining the surface
  const auto & IVmax = IV.colwise().maxCoeff();
  const auto & IVmin = IV.colwise().minCoeff();
  const double bbd = (IVmax-IVmin).norm();
  const double r = bbd/2.;
  const auto & IVmid = 0.5*(IVmax + IVmin);
  // Supposedly the implict needs to evaluate to <0 at cmid...
  // http://doc.cgal.org/latest/Surface_mesher/classCGAL_1_1Implicit__surface__3.html
  Point_3 cmid(IVmid(0),IVmid(1),IVmid(2));
  Function fun;
  switch(sign_type)
  {
    default:
      assert(false && "Unknown SignedDistanceType");
    case SIGNED_DISTANCE_TYPE_UNSIGNED:
      fun = 
        [&tree,&IV,&IF,&level](const Point_3 & q) -> FT
        {
          int i;
          RowVector3d c;
          const double sd = tree.squared_distance(
            IV,IF,RowVector3d(q.x(),q.y(),q.z()),i,c);
          return sd-level;
        };
    case SIGNED_DISTANCE_TYPE_DEFAULT:
    case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
      fun = 
        [&tree,&IV,&IF,&hier,&level](const Point_3 & q) -> FT
        {
          const double sd = signed_distance_winding_number(
            tree,IV,IF,hier,Vector3d(q.x(),q.y(),q.z()));
          return sd-level;
        };
      break;
    case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
      fun = [&tree,&IV,&IF,&FN,&VN,&EN,&EMAP,&level](const Point_3 & q) -> FT
        {
          const double sd = 
            igl::signed_distance_pseudonormal(
              tree,IV,IF,FN,VN,EN,EMAP,RowVector3d(q.x(),q.y(),q.z()));
          return sd- level;
        };
      break;
  }
  Sphere_3 bounding_sphere(cmid, (r+level)*(r+level));
  Surface_3 surface(fun,bounding_sphere);
  CGAL::Surface_mesh_default_criteria_3<Tr> 
    criteria(angle_bound,radius_bound,distance_bound);
  // meshing surface
  CGAL::make_surface_mesh(c2t3, surface, criteria, CGAL::Manifold_tag());
  // complex to (V,F)
  return igl::copyleft::cgal::complex_to_mesh(c2t3,V,F);
}
Ejemplo n.º 3
0
Archivo: Lemke.cpp Proyecto: hsu/dart
int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q, Eigen::VectorXd& _z)
{
    int n = _q.size();

    const double zer_tol = 1e-5;
    const double piv_tol = 1e-8;
    int maxiter = 1000;
    int err = 0;

    if (_q.minCoeff() > 0)
    {
        //		    LOG(INFO) << "Trivial solution exists.";
        _z = Eigen::VectorXd::Zero(n);
        return err;
    }

    _z = Eigen::VectorXd::Zero(2 * n);
    int iter = 0;
    double theta = 0;
    double ratio = 0;
    int leaving  = 0;
    Eigen::VectorXd Be = Eigen::VectorXd::Constant(n, 1);
    Eigen::VectorXd x = _q;
    std::vector<int> bas;
    std::vector<int> nonbas;

    int t = 2 * n + 1;
    int entering = t;

    bas.clear();
    nonbas.clear();

    for (int i = 0; i < n; ++i)
    {
        bas.push_back(i);
    }

    Eigen::MatrixXd B = -Eigen::MatrixXd::Identity(n, n);

    for (int i = 0; i < bas.size(); ++i) {
        B.col(bas[i]) = _M.col(bas[i]);
    }

    x = -B.partialPivLu().solve(_q);

    Eigen::VectorXd minuxX = -x;
    int lvindex;
    double tval = minuxX.maxCoeff(&lvindex);
    leaving = bas[lvindex];
    bas[lvindex] = t;

    Eigen::VectorXd U = Eigen::VectorXd::Zero(n);
    for (int i = 0; i < n; ++i)
    {
        if (x[i] < 0)
            U[i] = 1;
    }
    Be = -(B * U);
    x += tval * U;
    x[lvindex] = tval;
    B.col(lvindex) = Be;

    for (iter = 0; iter < maxiter; ++iter)
    {
        if (leaving == t)
        {
            break;
        }
        else if (leaving < n)
        {
            entering = n + leaving;
            Be = Eigen::VectorXd::Zero(n);
            Be[leaving] = -1;
        }
        else
        {
            entering = leaving - n;
            Be = _M.col(entering);
        }

        Eigen::VectorXd d = B.partialPivLu().solve(Be);

        std::vector<int> j;
        for (int i = 0; i < n; ++i)
        {
            if (d[i] > piv_tol)
                j.push_back(i);
        }
        if (j.empty())
        {
            // 			    err = 2;
            break;
        }

        int jSize = static_cast<int>(j.size());
        Eigen::VectorXd minRatio(jSize);
        for (int i = 0; i < jSize; ++i)
        {
            minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]];
        }
        double theta = minRatio.minCoeff();

        std::vector<int> tmpJ;
        std::vector<double> tmpMinRatio;
        for (int i = 0; i < jSize; ++i)
        {
            if (x[j[i]] / d[j[i]] <= theta)
            {
                tmpJ.push_back(j[i]);
                tmpMinRatio.push_back(minRatio[i]);
            }
        }
        //             if (tmpJ.empty())
        //             {
        //                 LOG(WARNING) << "tmpJ should never be empty!!!";
        //                 LOG(WARNING) << "dumping data:";
        //                 LOG(WARNING) << "theta:" << theta;
        //                 for (int i = 0; i < jSize; ++i)
        //                 {
        //                     LOG(WARNING) << "x(" << j[i] << "): " << x[j[i]] << "d: " << d[j[i]];
        //                 }
        //             }

        j = tmpJ;
        jSize = static_cast<int>(j.size());
        if (jSize == 0)
        {
            err = 4;
            break;
        }
        lvindex = -1;

        for (int i = 0; i < jSize; ++i)
        {
            if (bas[j[i]] == t)
                lvindex = i;
        }
        if (lvindex != -1)
        {
            lvindex = j[lvindex];
        }
        else
        {
            theta = tmpMinRatio[0];
            lvindex = 0;

            for (int i = 0; i < jSize; ++i)
            {
                if (tmpMinRatio[i]-theta > piv_tol)
                {
                    theta = tmpMinRatio[i];
                    lvindex = i;
                }
            }
            lvindex = j[lvindex];

        }

        leaving = bas[lvindex];

        ratio = x[lvindex] / d[lvindex];

        bool bDiverged = false;
        for (int i = 0; i < n; ++i)
        {
            if (isnan(x[i]) || isinf(x[i]))
            {
                bDiverged = true;
                break;
            }
        }
        if (bDiverged)
        {
            err = 4;
            break;
        }
        x = x - ratio * d;
        x[lvindex] = ratio;
        B.col(lvindex) = Be;
        bas[lvindex] = entering;

    }

    if (iter >= maxiter && leaving != t)
        err = 1;

    if (err == 0)
    {
        for (size_t i = 0; i < bas.size(); ++i) {
            if (bas[i] < _z.size()) {
                _z[bas[i]] = x[i];
            }
        }

        Eigen::VectorXd realZ = _z.segment(0, n);
        _z = realZ;

        if (!validate(_M, _z, _q))
        {
            // 				_z = VectorXd::Zero(n);
            err = 3;
        }
    }
    else
    {
        _z = Eigen::VectorXd::Zero(n); //solve failed, return a 0 vector
    }

    // 	    if (err == 1)
    // 		    LOG(ERROR) << "LCP Solver: Iterations exceeded limit";
    // 	    else if (err == 2)
    // 		    LOG(ERROR) << "LCP Solver: Unbounded ray";
    //         else if (err == 3)
    //             LOG(ERROR) << "LCP Solver: Solver converged with numerical issues. Validation failed.";
    //         else if (err == 4)
    //             LOG(ERROR) << "LCP Solver: Iteration diverged.";

    return err;
}
Ejemplo n.º 4
0
/** \todo Document this rather complex function */
void MetaParametersLWR::getCentersAndWidths(const VectorXd& min, const VectorXd& max, Eigen::MatrixXd& centers, Eigen::MatrixXd& widths) const
{
  int n_dims = getExpectedInputDim();
  assert(min.size()==n_dims);
  assert(max.size()==n_dims);
    
  vector<VectorXd> centers_per_dim_local(n_dims); 
  if (!centers_per_dim_.empty())
  {
    centers_per_dim_local = centers_per_dim_;
  }
  else
  {
    // Centers are not know yet, compute them based on min and max
    for (int i_dim=0; i_dim<n_dims; i_dim++)
      centers_per_dim_local[i_dim] = VectorXd::LinSpaced(n_bfs_per_dim_[i_dim],min[i_dim],max[i_dim]);
  }
  
  // Determine the widths from the centers (separately for each dimension)
  vector<VectorXd> widths_per_dim_local(n_dims); 
  for (int i_dim=0; i_dim<n_dims; i_dim++)
  {
    VectorXd cur_centers = centers_per_dim_local[i_dim]; // Abbreviation for convenience
    int n_centers = cur_centers.size();
    VectorXd cur_widths(n_centers);

    if (n_centers==1)
    {
      cur_widths[0] = 1.0;
    }
    else if (n_centers==2)
    {
      cur_widths.fill(sqrt(overlap_*(cur_centers[1]-cur_centers[0])));
    }
    else
    {
      cur_widths[0] = sqrt(overlap_*fabs(cur_centers[1]-cur_centers[0]));
      cur_widths[n_centers-1] = sqrt(overlap_*fabs(cur_centers[n_centers-1]-cur_centers[n_centers-2]));
      for (int cc=1; cc<n_centers-1; cc++)
      {
        double width_left  = sqrt(overlap_*fabs(cur_centers[cc-1]-cur_centers[cc]));
        double width_right = sqrt(overlap_*fabs(cur_centers[cc+1]-cur_centers[cc]));
        // Recommended width is the average of the recommended widths for left and right
        cur_widths[cc] = 0.5*(width_left+width_right);
      }
    }
    widths_per_dim_local[i_dim] = cur_widths;
  }

  VectorXd digit_max(n_dims);
  int n_centers = 1;
  for (int i_dim=0; i_dim<n_dims; i_dim++)
  {
    n_centers = n_centers*centers_per_dim_local[i_dim].size();
    digit_max[i_dim] = centers_per_dim_local[i_dim].size();
  }
  VectorXi digit = VectorXi::Zero(n_dims);
  
  centers.resize(n_centers,n_dims);
  widths.resize(n_centers,n_dims);
  int i_center=0;

  while (digit[0]<digit_max(0))
  {
    for (int i_dim=0; i_dim<n_dims; i_dim++)
    {
      centers(i_center,i_dim) = centers_per_dim_local[i_dim][digit[i_dim]];
      widths(i_center,i_dim) = widths_per_dim_local[i_dim][digit[i_dim]];
    }
    i_center++;
  
    // Increment last digit by one
    digit[n_dims-1]++;
    for (int i_dim=n_dims-1; i_dim>0; i_dim--)
    {
      if (digit[i_dim]>=digit_max[i_dim])
      {
        digit[i_dim] = 0;
        digit[i_dim-1]++;
      }
    }
  }
  
}
Ejemplo n.º 5
0
void mouse_drag(int mouse_x, int mouse_y)
{
  using namespace igl;
  using namespace std;
  using namespace Eigen;

  if(is_rotating)
  {
    glutSetCursor(GLUT_CURSOR_CYCLE);
    Quaterniond q;
    switch(rotation_type)
    {
      case ROTATION_TYPE_IGL_TRACKBALL:
      {
        // Rotate according to trackball
        igl::trackball<double>(
          width,
          height,
          2.0,
          down_camera.m_rotation_conj.coeffs().data(),
          down_x,
          down_y,
          mouse_x,
          mouse_y,
          q.coeffs().data());
          break;
      }
      case ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP:
      {
        // Rotate according to two axis valuator with fixed up vector
        two_axis_valuator_fixed_up(
          width, height,
          2.0,
          down_camera.m_rotation_conj,
          down_x, down_y, mouse_x, mouse_y,
          q);
        break;
      }
      default:
        break;
    }
    camera.orbit(q.conjugate());
  }

  if(is_dragging)
  {
    push_scene();
    push_object();
    if(new_leaf_on_drag)
    {
      assert(s.C.size() >= 1);
      // one new node
      s.C.conservativeResize(s.C.rows()+1,3);
      const int nc = s.C.rows();
      assert(s.sel.size() >= 1);
      s.C.row(nc-1) = s.C.row(s.sel(0));
      // one new bone
      s.BE.conservativeResize(s.BE.rows()+1,2);
      s.BE.row(s.BE.rows()-1) = RowVector2i(s.sel(0),nc-1);
      // select just last node
      s.sel.resize(1,1);
      s.sel(0) = nc-1;
      // reset down_C
      down_C = s.C;
      new_leaf_on_drag = false;
    }
    if(new_root_on_drag)
    {
      // two new nodes
      s.C.conservativeResize(s.C.rows()+2,3);
      const int nc = s.C.rows();
      Vector3d obj;
      int nhits = unproject_in_mesh(mouse_x,height-mouse_y,ei,obj);
      if(nhits == 0)
      {
        Vector3d pV_mid = project(Vcen);
        obj = unproject(Vector3d(mouse_x,height-mouse_y,pV_mid(2)));
      }
      s.C.row(nc-2) = obj;
      s.C.row(nc-1) = obj;
      // select last node
      s.sel.resize(1,1);
      s.sel(0) = nc-1;
      // one new bone
      s.BE.conservativeResize(s.BE.rows()+1,2);
      s.BE.row(s.BE.rows()-1) = RowVector2i(nc-2,nc-1);
      // reset down_C
      down_C = s.C;
      new_root_on_drag = false;
    }
    double z = 0;
    Vector3d obj,win;
    int nhits = unproject_in_mesh(mouse_x,height-mouse_y,ei,obj);
    project(obj,win);
    z = win(2);

    for(int si = 0;si<s.sel.size();si++)
    {
      const int c = s.sel(si);
      Vector3d pc = project((RowVector3d) down_C.row(c));
      pc(0) += mouse_x-down_x;
      pc(1) += (height-mouse_y)-(height-down_y);
      if(nhits > 0)
      {
        pc(2) = z;
      }
      s.C.row(c) = unproject(pc);
    }
    pop_object();
    pop_scene();
  }

  glutPostRedisplay();
}
Ejemplo n.º 6
0
// Initialize colors to a boring green
void init_C()
{
  C.col(0).setConstant(0.4);
  C.col(1).setConstant(0.8);
  C.col(2).setConstant(0.3);
}
Ejemplo n.º 7
0
bool GaussianSet::setDensityMatrix(const Eigen::MatrixXd &m)
{
  m_density.resize(m.rows(), m.cols());
  m_density = m;
  return true;
}
Ejemplo n.º 8
0
/*! \fn inline void symmetryBlocking(Eigen::MatrixXd & matrix, int cavitySize, int ntsirr, int nr_irrep)
 *  \param[out] matrix the matrix to be block-diagonalized
 *  \param[in]  cavitySize the size of the cavity (size of the matrix)
 *  \param[in]  ntsirr     the size of the irreducible portion of the cavity (size of the blocks)
 *  \param[in]  nr_irrep   the number of irreducible representations (number of blocks)
 */
inline void symmetryBlocking(Eigen::MatrixXd & matrix, size_t cavitySize, int ntsirr,
                             int nr_irrep)
{
    // This function implements the simmetry-blocking of the PCM
    // matrix due to point group symmetry as reported in:
    // L. Frediani, R. Cammi, C. S. Pomelli, J. Tomasi and K. Ruud, J. Comput.Chem. 25, 375 (2003)
    // u is the character table for the group (t in the paper)
    Eigen::MatrixXd u = Eigen::MatrixXd::Zero(nr_irrep, nr_irrep);
    for (int i = 0; i < nr_irrep; ++i) {
        for (int j = 0; j < nr_irrep; ++j) {
            u(i, j) = parity(i&j);
        }
    }
    // Naming of indices:
    //     a, b, c, d   run over the total size of the cavity (N)
    //     i, j, k, l   run over the number of irreps (n)
    //     p, q, r, s   run over the irreducible size of the cavity (N/n)
    // Instead of forming U (T in the paper) and then perform the dense
    // multiplication, we multiply block-by-block using just the u matrix.
    //      matrix = U * matrix * Ut; U * Ut = Ut * U = id
    // First half-transformation, i.e. first_half = matrix * Ut
    Eigen::MatrixXd first_half = Eigen::MatrixXd::Zero(cavitySize, cavitySize);
    for (int i = 0; i < nr_irrep; ++i) {
        int ioff = i * ntsirr;
        for (int k = 0; k < nr_irrep; ++k) {
            int koff = k * ntsirr;
            for (int j = 0; j < nr_irrep; ++j) {
                int joff = j * ntsirr;
                double ujk = u(j, k) / nr_irrep;
                for (int p = 0; p < ntsirr; ++p) {
                    int a = ioff + p;
                    for (int q = 0; q < ntsirr; ++q) {
                        int b = joff + q;
                        int c = koff + q;
                        first_half(a, c) += matrix(a, b) * ujk;
                    }
                }
            }
        }
    }
    // Second half-transformation, i.e. matrix = U * first_half
    matrix.setZero(cavitySize, cavitySize);
    for (int i = 0; i < nr_irrep; ++i) {
        int ioff = i * ntsirr;
        for (int k = 0; k < nr_irrep; ++k) {
            int koff = k * ntsirr;
            for (int j = 0; j < nr_irrep; ++j) {
                int joff = j * ntsirr;
                double uij = u(i, j);
                for (int p = 0; p < ntsirr; ++p) {
                    int a = ioff + p;
                    int b = joff + p;
                    for (int q = 0; q < ntsirr; ++q) {
                        int c = koff + q;
                        matrix(a, c) += uij * first_half(b, c);
                    }
                }
            }
        }
    }
    // Traverse the matrix and discard numerical zeros
    for (size_t a = 0; a < cavitySize; ++a) {
        for (size_t b = 0; b < cavitySize; ++b) {
            if (numericalZero(matrix(a, b))) {
                matrix(a, b) = 0.0;
            }
        }
    }
}
Ejemplo n.º 9
0
/** unittest for oapackage
 *
 * Returns UNITTEST_SUCCESS if all tests are ok.
 *
 */
int oaunittest (int verbose, int writetests = 0, int randval = 0) {
        double t0 = get_time_ms ();
        const char *bstr = "OA unittest";
        cprintf (verbose, "%s: start\n", bstr);

        srand (randval);

        int allgood = UNITTEST_SUCCESS;

        Combinations::initialize_number_combinations (20);

        /* constructors */
        {
                cprintf (verbose, "%s: interaction matrices\n", bstr);

                array_link al = exampleArray (2);
                Eigen::MatrixXd m1 = array2xfeigen (al);
                Eigen::MatrixXd m2 = arraylink2eigen (array2xf (al));

                Eigen::MatrixXd dm = m1 - m2;
                int sum = dm.sum ();

                myassert (sum == 0, "unittest error: construction of interaction matrices\n");
        }

		cprintf(verbose, "%s: reduceConferenceTransformation\n", bstr);
		myassert(unittest_reduceConferenceTransformation()==0, "unittest unittest_reduceConferenceTransformation failed");

        /* constructors */
        {
                cprintf (verbose, "%s: array manipulation operations\n", bstr);

                test_array_manipulation (verbose);
        }

        /* double conference matrices */
        {
                cprintf (verbose, "%s: double conference matrices\n", bstr);

                array_link al = exampleArray (36, verbose);
                myassert (al.is_conference (2), "check on double conference design type");

                myassert (testLMC0checkDC (al, verbose >= 2), "testLMC0checkDC");

        }

        /* conference matrices */
        {
                cprintf (verbose, "%s: conference matrices\n", bstr);

                int N = 4;
                conference_t ctype (N, N, 0);

                arraylist_t kk;
                array_link al = ctype.create_root ();
                kk.push_back (al);

                for (int extcol = 2; extcol < N; extcol++) {
                        kk = extend_conference (kk, ctype, 0);
                }
                myassert (kk.size () == 1, "unittest error: conference matrices for N=4\n");
        }

        {
                cprintf (verbose, "%s: generators for conference matrix extensions\n", bstr);
                test_conference_candidate_generators (verbose);
        }

        {
                cprintf (verbose, "%s: conference matrix Fvalues\n", bstr);
                array_link al = exampleArray (22, 0);
                if (verbose >= 2)
                        al.show ();
                if (0) {
                        std::vector< int > f3 = al.FvaluesConference (3);
                        if (verbose >= 2) {
                                printf ("F3: ");
                                display_vector (f3);
                                printf ("\n");
                        }
                }

                const int N = al.n_rows;
                jstructconference_t js (N, 4);
                std::vector< int > f4 = al.FvaluesConference (4);
                std::vector< int > j4 = js.Jvalues ();

                if (verbose >= 2) {
                        printf ("j4: ");
                        display_vector (j4);
                        printf ("\n");
                        printf ("F4: ");
                        display_vector (f4);
                        printf ("\n");
                }

                myassert (j4[0] == 28, "unittest error: conference matricex F values: j4[0]\n");
                myassert (f4[0] == 0, "unittest error: conference matricex F values: f4[0] \n");
                myassert (f4[1] == 0, "unittest error: conference matricex F values: j4[1]\n");
        }

        {
                cprintf (verbose, "%s: LMC0 check for arrays in C(4, 3)\n", bstr);

                array_link al = exampleArray (28, 1);
                if (verbose >= 2)
                        al.showarray ();
                lmc_t r = LMC0check (al, verbose);
                if (verbose >= 2)
                        printf ("LMC0check: result %d\n", r);
                myassert (r >= LMC_EQUAL, "LMC0 check\n");

                al = exampleArray (29, 1);
                if (verbose >= 2)
                        al.showarray ();
                r = LMC0check (al, verbose);
                if (verbose >= 2)
                        printf ("LMC0check: result %d (LMC_LESS %d)\n", r, LMC_LESS);
                myassert (r == LMC_LESS, "LMC0 check of example array 29\n");
        }

        {
                cprintf (verbose, "%s: LMC0 check\n", bstr);

                array_link al = exampleArray (31, 1);
                if (verbose >= 2)
                        al.showarray ();
                conference_transformation_t T (al);

                for (int i = 0; i < 80; i++) {
                        T.randomize ();
                        array_link alx = T.apply (al);

                        lmc_t r = LMC0check (alx, verbose);

                        if (verbose >= 2) {
                                printfd ("%d: transformed array: r %d\n", i, r);
                                alx.showarray ();
                        }
                        if (alx == al)
                                myassert (r >= LMC_EQUAL, "result should be LMC_MORE\n");
                        else {
                                myassert (r == LMC_LESS, "result should be LMC_LESS\n");
                        }
                }
        }

        {
                cprintf (verbose, "%s: random transformation for conference matrices\n", bstr);

                array_link al = exampleArray (19, 1);
                conference_transformation_t T (al);
                // T.randomizerowflips();
                T.randomize ();

                conference_transformation_t Ti = T.inverse ();
                array_link alx = Ti.apply (T.apply (al));

                if (0) {
                        printf ("input array:\n");
                        al.showarray ();
                        T.show ();
                        printf ("transformed array:\n");
                        T.apply (al).showarray ();
                        Ti.show ();
                        alx.showarray ();
                }

                myassert (alx == al, "transformation of conference matrix\n");
        }

        /* constructors */
        {
                cprintf (verbose, "%s: constructors\n", bstr);

                array_transformation_t t;
                conference_transformation_t ct;
        }

        /* J-characteristics */
        {
                cprintf (verbose, "%s: J-characteristics\n", bstr);

                array_link al = exampleArray (8, 1);

                const int mm[] = {-1, -1, 0, 0, 8, 16, 0, -1};

                for (int jj = 2; jj < 7; jj++) {
                        std::vector< int > jx = al.Jcharacteristics (jj);
                        int j5max = vectormax (jx, 0);
                        if (verbose >= 2) {
                                printf ("oaunittest: jj %d: j5max %d\n", jj, j5max);
                        }

                        if (j5max != mm[jj]) {
                                printfd ("j5max %d (should be %d)\n", j5max, mm[jj]);
                                allgood = UNITTEST_FAIL;
                                return allgood;
                        }
                }
        }
        {
                cprintf (verbose, "%s: array transformations\n", bstr);

                const int N = 9;
                const int t = 3;
                arraydata_t adataX (3, N, t, 4);

                array_link al (adataX.N, adataX.ncols, -1);
                al.create_root (adataX);

                if (checkTransformationInverse (al))
                        allgood = UNITTEST_FAIL;

                if (checkTransformationComposition (al, verbose >= 2))
                        allgood = UNITTEST_FAIL;

                al = exampleArray (5, 1);
                if (checkTransformationInverse (al))
                        allgood = UNITTEST_FAIL;

                if (checkTransformationComposition (al))
                        allgood = UNITTEST_FAIL;

                for (int i = 0; i < 15; i++) {
                        al = exampleArray (18, 0);
                        if (checkConferenceComposition (al))
                                allgood = UNITTEST_FAIL;
                        if (checkConferenceInverse (al))
                                allgood = UNITTEST_FAIL;
                        al = exampleArray (19, 0);
                        if (checkConferenceComposition (al))
                                allgood = UNITTEST_FAIL;
                        if (checkConferenceInverse (al))
                                allgood = UNITTEST_FAIL;
                }
        }

        {
                cprintf (verbose, "%s: rank \n", bstr);

                const int idx[10] = {0, 1, 2, 3, 4, 6, 7, 8, 9};
                const int rr[10] = {4, 11, 13, 18, 16, 4, 4, 29, 29};
                for (int ii = 0; ii < 9; ii++) {
                        array_link al = exampleArray (idx[ii], 0);
                        myassert (al.is2level (), "unittest error: input array is not 2-level\n");

                        int r = arrayrankColPivQR (array2xf (al));

                        int r3 = (array2xf (al)).rank ();
                        myassert (r == r3, "unittest error: rank of array");

                        if (verbose >= 2) {
                                al.showarray ();
                                printf ("unittest: rank of array %d: %d\n", idx[ii], r);
                        }

                        myassert (rr[ii] == r, "unittest error: rank of example matrix\n");
                }
        }

        {
                cprintf (verbose, "%s: Doptimize \n", bstr);
                const int N = 40;
                const int t = 0;
                arraydata_t arrayclass (2, N, t, 6);
                std::vector< double > alpha (3);
                alpha[0] = 1;
                alpha[1] = 1;
                alpha[2] = 0;
                int niter = 5000;
                double t00 = get_time_ms ();
                DoptimReturn rr = Doptimize (arrayclass, 10, alpha, 0, DOPTIM_AUTOMATIC, niter);

                array_t ss[7] = {3, 3, 2, 2, 2, 2, 2};
                arraydata_t arrayclassmixed (ss, 36, t, 5);
                rr = Doptimize (arrayclassmixed, 10, alpha, 0, DOPTIM_AUTOMATIC, niter);

                cprintf (verbose, "%s: Doptimize time %.3f [s] \n", bstr, get_time_ms () - t00);
        }

        {
                cprintf (verbose, "%s: J-characteristics for conference matrix\n", bstr);

                array_link al = exampleArray (19, 0);
                std::vector< int > j2 = Jcharacteristics_conference (al, 2);
                std::vector< int > j3 = Jcharacteristics_conference (al, 3);

                myassert (j2[0] == 0, "j2 value incorrect");
                myassert (j2[1] == 0, "j2 value incorrect");
                myassert (std::abs (j3[0]) == 1, "j3 value incorrect");

                if (verbose >= 2) {
                        al.showarray ();
                        printf ("j2: ");
                        display_vector (j2);
                        printf ("\n");
                        printf ("j3: ");
                        display_vector (j3);
                        printf ("\n");
                }
        }

        {
                // test PEC sequence
                cprintf (verbose, "%s: PEC sequence\n", bstr);
                for (int ii = 0; ii < 5; ii++) {
                        array_link al = exampleArray (ii, 0);
                        std::vector< double > pec = PECsequence (al);
                        printf ("oaunittest: PEC for array %d: ", ii);
                        display_vector (pec);
                        printf (" \n");
                }
        }

        {
                cprintf (verbose, "%s: D-efficiency test\n", bstr);
                //  D-efficiency near-zero test
                {
                        array_link al = exampleArray (14);
                        double D = al.Defficiency ();
                        std::vector< double > dd = al.Defficiencies ();
                        printf ("D %f, D (method 2) %f\n", D, dd[0]);
                        assert (fabs (D - dd[0]) < 1e-4);
                }
                {
                        array_link al = exampleArray (15);
                        double D = al.Defficiency ();
                        std::vector< double > dd = al.Defficiencies ();
                        printf ("D %f, D (method 2) %f\n", D, dd[0]);
                        assert (fabs (D - dd[0]) < 1e-4);
                        assert (fabs (D - 0.335063) < 1e-3);
                }
        }

        arraydata_t adata (2, 20, 2, 6);
        OAextend oaextendx;
        oaextendx.setAlgorithm ((algorithm_t)MODE_ORIGINAL, &adata);

        std::vector< arraylist_t > aa (adata.ncols + 1);
        printf ("OA unittest: create root array\n");
        create_root (&adata, aa[adata.strength]);

        /** Test extend of arrays **/
        {
                cprintf (verbose, "%s: extend arrays\n", bstr);

                setloglevel (SYSTEM);

                for (int kk = adata.strength; kk < adata.ncols; kk++) {
                        aa[kk + 1] = extend_arraylist (aa[kk], adata, oaextendx);
                        printf ("  extend: column %d->%d: %ld->%ld arrays\n", kk, kk + 1, aa[kk].size (),
                                aa[kk + 1].size ());
                }

                if (aa[adata.ncols].size () != 75) {
                        printf ("extended ?? to %d arrays\n", (int)aa[adata.ncols].size ());
                }
                myassert (aa[adata.ncols].size () == 75, "number of arrays is incorrect");

                aa[adata.ncols].size ();
                setloglevel (QUIET);
        }

        {
                cprintf (verbose, "%s: test LMC check\n", bstr);

                array_link al = exampleArray (1, 1);

                lmc_t r = LMCcheckOriginal (al);

                myassert (r != LMC_LESS, "LMC check of array in normal form");

                for (int i = 0; i < 20; i++) {
                        array_link alx = al.randomperm ();
                        if (alx == al)
                                continue;
                        lmc_t r = LMCcheckOriginal (alx);

                        myassert (r == LMC_LESS, "randomized array cannot be in minimal form");
                }
        }

        {
                /** Test dof **/
                cprintf (verbose, "%s: test delete-one-factor reduction\n", bstr);

                array_link al = exampleArray (4);
                cprintf (verbose >= 2, "LMC: \n");
                al.reduceLMC ();
                cprintf (verbose >= 2, "DOP: \n");
                al.reduceDOP ();
        }

        arraylist_t lst;

        {
                /** Test different methods **/
                cprintf (verbose, "%s: test 2 different methods\n", bstr);

                const int s = 2;
                arraydata_t adata (s, 32, 3, 10);
                arraydata_t adata2 (s, 32, 3, 10);
                OAextend oaextendx;
                oaextendx.setAlgorithm ((algorithm_t)MODE_ORIGINAL, &adata);
                OAextend oaextendx2;
                oaextendx2.setAlgorithm ((algorithm_t)MODE_LMC_2LEVEL, &adata2);

                printf ("OA unittest: test 2-level algorithm on %s\n", adata.showstr ().c_str ());
                std::vector< arraylist_t > aa (adata.ncols + 1);
                create_root (&adata, aa[adata.strength]);
                std::vector< arraylist_t > aa2 (adata.ncols + 1);
                create_root (&adata, aa2[adata.strength]);

                setloglevel (SYSTEM);

                for (int kk = adata.strength; kk < adata.ncols; kk++) {
                        aa[kk + 1] = extend_arraylist (aa[kk], adata, oaextendx);
                        aa2[kk + 1] = extend_arraylist (aa2[kk], adata2, oaextendx2);
                        printf ("  extend: column %d->%d: %ld->%ld arrays, 2-level method %ld->%ld arrays\n", kk,
                                kk + 1, (long) aa[kk].size (), (long)aa[kk + 1].size (), aa2[kk].size (), aa2[kk + 1].size ());

                        if (aa[kk + 1] != aa2[kk + 1]) {
                                printf ("oaunittest: error: 2-level algorithm unequal to original algorithm\n");
                                exit (1);
                        }
                }
                setloglevel (QUIET);

                lst = aa[8];
        }

        {
                cprintf (verbose, "%s: rank calculation using rankStructure\n", bstr);

                for (int i = 0; i < 27; i++) {
                        array_link al = exampleArray (i, 0);
                        if (al.n_columns < 5)
                                continue;
                        al = exampleArray (i, 1);

                        rankStructure rs;
                        rs.verbose = 0;
                        int r = array2xf (al).rank ();
                        int rc = rs.rankxf (al);
                        if (verbose >= 2) {
                                printf ("rank of example array %d: %d %d\n", i, r, rc);
                                if (verbose >= 3) {
                                        al.showproperties ();
                                }
                        }
                        myassert (r == rc, "rank calculations");
                }
        }
        {
                cprintf (verbose, "%s: test dtable creation\n", bstr);

                for (int i = 0; i < 4; i++) {
                        array_link al = exampleArray (5);
                        array_link dtable = createJdtable (al);
                }
        }

        {
                cprintf (verbose, "%s: test Pareto calculation\n", bstr);
                double t0x = get_time_ms ();

                int nn = lst.size ();
                for (int k = 0; k < 5; k++) {
                        for (int i = 0; i < nn; i++) {
                                lst.push_back (lst[i]);
                        }
                }
                Pareto< mvalue_t< long >, long > r = parsePareto (lst, 1);
                cprintf (verbose, "%s: test Pareto %d/%d: %.3f [s]\n", bstr, r.number (), r.numberindices (),
                         (get_time_ms () - t0x));
        }

        {
                cprintf (verbose, "%s: check reduction transformation\n", bstr);
                array_link al = exampleArray (6).reduceLMC ();

                arraydata_t adata = arraylink2arraydata (al);
                LMCreduction_t reduction (&adata);
                reduction.mode = OA_REDUCE;

                reduction.init_state = COPY;
                OAextend oaextend;
                oaextend.setAlgorithm (MODE_ORIGINAL, &adata);
                array_link alr = al.randomperm ();

                array_link al2 = reduction.transformation->apply (al);

                lmc_t tmp = LMCcheck (alr, adata, oaextend, reduction);

                array_link alx = reduction.transformation->apply (alr);

                bool c = alx == al;
                if (!c) {
                        printf ("oaunittest: error: reduction of randomized array failed!\n");
                        printf ("-- al \n");
                        al.showarraycompact ();
                        printf ("-- alr \n");
                        alr.showarraycompact ();
                        printf ("-- alx \n");
                        alx.showarraycompact ();
                        allgood = UNITTEST_FAIL;
                }
        }

        {
                cprintf (verbose, "%s: reduce randomized array\n", bstr);
                array_link al = exampleArray (3);

                arraydata_t adata = arraylink2arraydata (al);
                LMCreduction_t reduction (&adata);

                for (int ii = 0; ii < 50; ii++) {
                        reduction.transformation->randomize ();
                        array_link al2 = reduction.transformation->apply (al);

                        array_link alr = al2.reduceLMC ();
                        if (0) {
                                printf ("\n reduction complete:\n");
                                al2.showarray ();
                                printf ("	--->\n");
                                alr.showarray ();
                        }
                        bool c = (al == alr);
                        if (!c) {
                                printf ("oaunittest: error: reduction of randomized array failed!\n");
                                allgood = UNITTEST_FAIL;
                        }
                }
        }

        /* Calculate symmetry group */
        {
                cprintf (verbose, "%s: calculate symmetry group\n", bstr);

                array_link al = exampleArray (2);
                symmetry_group sg = al.row_symmetry_group ();
                assert (sg.permsize () == sg.permsize_large ().toLong ());

                // symmetry_group
                std::vector< int > vv;
                vv.push_back (0);
                vv.push_back (0);
                vv.push_back (1);
                symmetry_group sg2 (vv);
                assert (sg2.permsize () == 2);
                if (verbose >= 2)
                        printf ("sg2: %ld\n", sg2.permsize ());
                assert (sg2.ngroups == 2);
        }

        /* Test efficiencies */
        {
                cprintf (verbose, "%s: efficiencies\n", bstr);

                std::vector< double > d;
                int vb = 1;

                array_link al;
                if (1) {
                        al = exampleArray (9, vb);
                        al.showproperties ();
                        d = al.Defficiencies (0, 1);
                        if (verbose >= 2)
                                printf ("  efficiencies: D %f Ds %f D1 %f Ds0 %f\n", d[0], d[1], d[2], d[3]);
                        if (fabs (d[0] - al.Defficiency ()) > 1e-10) {
                                printf ("oaunittest: error: Defficiency not good!\n");
                                allgood = UNITTEST_FAIL;
                        }
                }
                al = exampleArray (8, vb);
                al.showproperties ();
                d = al.Defficiencies ();
                if (verbose >= 2)
                        printf ("  efficiencies: D %f Ds %f D1 %f\n", d[0], d[1], d[2]);
                if (fabs (d[0] - al.Defficiency ()) > 1e-10) {
                        printf ("oaunittest: error: Defficiency of examlple array 8 not good!\n");
                }

                al = exampleArray (13, vb);
                if (verbose >= 3) {
                        al.showarray ();
                        al.showproperties ();
                }
                d = al.Defficiencies (0, 1);
                if (verbose >= 2)
                        printf ("  efficiencies: D %f Ds %f D1 %f\n", d[0], d[1], d[2]);

                if ((fabs (d[0] - 0.939014) > 1e-4) || (fabs (d[3] - 0.896812) > 1e-4) || (fabs (d[2] - 1) > 1e-4)) {
                        printf ("ERROR: D-efficiencies of example array 13 incorrect! \n");
                        d = al.Defficiencies (2, 1);
                        printf ("  efficiencies: D %f Ds %f D1 %f Ds0 %f\n", d[0], d[1], d[2], d[3]);

                        allgood = UNITTEST_FAIL;
                        exit (1);
                }

                for (int ii = 11; ii < 11; ii++) {
                        printf ("ii %d: ", ii);
                        al = exampleArray (ii, vb);
                        al.showarray ();
                        al.showproperties ();

                        d = al.Defficiencies ();
                        if (verbose >= 2)
                                printf ("  efficiencies: D %f Ds %f D1 %f\n", d[0], d[1], d[2]);
                }
        }
        {
                cprintf (verbose, "%s: test robustness\n", bstr);

                array_link A (0, 8, 0);
                printf ("should return an error\n  ");
                A.Defficiencies ();

                A = array_link (1, 8, 0);
                printf ("should return an error\n  ");
                A.at (0, 0) = -2;
                A.Defficiencies ();
        }

        {
                cprintf (verbose, "%s: test nauty\n", bstr);

                array_link alr = exampleArray (7, 0);
                if (unittest_nautynormalform (alr, 1) == 0) {
                        printf ("oaunittest: error: unittest_nautynormalform returns an error!\n");
                }
        }

#ifdef HAVE_BOOST
        if (writetests) {
                cprintf (verbose, "OA unittest: reading and writing of files\n");

                boost::filesystem::path tmpdir = boost::filesystem::temp_directory_path ();
                boost::filesystem::path temp = boost::filesystem::unique_path ("test-%%%%%%%.oa");

                const std::string tempstr = (tmpdir / temp).native (); 

                if (verbose >= 2)
                        printf ("generate text OA file: %s\n", tempstr.c_str ());

                int nrows = 16;
                int ncols = 8;
                int narrays = 10;
                arrayfile_t afile (tempstr.c_str (), nrows, ncols, narrays, ATEXT);
                for (int i = 0; i < narrays; i++) {
                        array_link al (nrows, ncols, array_link::INDEX_DEFAULT);                        
                        afile.append_array (al);
                }
                afile.closefile ();

                arrayfile_t af (tempstr.c_str (), 0);
                std::cout << "  " << af.showstr () << std::endl;
                af.closefile ();

                // check read/write of binary file

                arraylist_t ll0;
                ll0.push_back (exampleArray (7));
                ll0.push_back (exampleArray (7).randomcolperm ());
                writearrayfile (tempstr.c_str (), ll0, ABINARY);
                arraylist_t ll = readarrayfile (tempstr.c_str ());
                myassert (ll0.size () == ll.size (), "read and write of arrays: size of list");
                for (size_t i = 0; i < ll0.size (); i++) {
                        myassert (ll0[i] == ll[i], "read and write of arrays: array unequal");
                }

                ll0.resize (0);
                ll0.push_back (exampleArray (24));
                writearrayfile (tempstr.c_str (), ll0, ABINARY_DIFFZERO);
                ll = readarrayfile (tempstr.c_str ());
                myassert (ll0.size () == ll.size (), "read and write of arrays: size of list");
                for (size_t i = 0; i < ll0.size (); i++) {
                        myassert (ll0[i] == ll[i], "read and write of arrays: array unequal");
                }
        }

#endif

        {
                cprintf (verbose, "OA unittest: test nauty\n");
                array_link al = exampleArray (5, 2);
                arraydata_t arrayclass = arraylink2arraydata (al);

                for (int i = 0; i < 20; i++) {
                        array_link alx = al;
                        alx.randomperm ();
                        array_transformation_t t1 = reduceOAnauty (al);
                        array_link alr1 = t1.apply (al);

                        array_transformation_t t2 = reduceOAnauty (alx);
                        array_link alr2 = t2.apply (alx);

                        if (alr1 != alr2)
                                printf ("oaunittest: error: Nauty reductions unequal!\n");
                        allgood = UNITTEST_FAIL;
                }
        }

        cprintf (verbose, "OA unittest: complete %.3f [s]!\n", (get_time_ms () - t0));
        cprintf (verbose, "OA unittest: also run ptest.py to perform checks!\n");

        if (allgood) {
                printf ("OA unittest: all tests ok\n");
                return UNITTEST_SUCCESS;
        } else {
                printf ("OA unittest: ERROR!\n");
                return UNITTEST_FAIL;
        }
}
Ejemplo n.º 10
0
void NewVizManager::animatePath(const ItompTrajectoryConstPtr& trajectory,
								const robot_state::RobotStatePtr& robot_state, bool is_best)
{
	if (!is_best)
        return;

    // marker array
	visualization_msgs::MarkerArray ma;
    std::vector<std::string> link_names = robot_model_->getMoveitRobotModel()->getLinkModelNames();
    std_msgs::ColorRGBA color = colors_[WHITE];
    color.a = 1.0;
	ros::Duration dur(3600.0);

    for (unsigned int point = 0; point < trajectory->getNumPoints(); ++point)
	{
		ma.markers.clear();
        const Eigen::MatrixXd mat = trajectory->getElementTrajectory(
                                        ItompTrajectory::COMPONENT_TYPE_POSITION,
                                        ItompTrajectory::SUB_COMPONENT_TYPE_JOINT)->getTrajectoryPoint(point);
		robot_state->setVariablePositions(mat.data());
        std::string ns = "frame_" + boost::lexical_cast<std::string>(point);
		robot_state->getRobotMarkers(ma, link_names, color, ns, dur);
        for (int i = 0; i < ma.markers.size(); ++i)
            ma.markers[i].mesh_use_embedded_materials = true;
        vis_marker_array_publisher_path_.publish(ma);
    }


    // MotionPlanning -> Planned Path -> trajectory
    moveit_msgs::DisplayTrajectory display_trajectory;

    int num_all_joints = robot_state->getVariableCount();

    ElementTrajectoryConstPtr joint_trajectory = trajectory->getElementTrajectory(ItompTrajectory::COMPONENT_TYPE_POSITION,
                ItompTrajectory::SUB_COMPONENT_TYPE_JOINT);
    robot_trajectory::RobotTrajectoryPtr response_trajectory = boost::make_shared<robot_trajectory::RobotTrajectory>(robot_model_->getMoveitRobotModel(), "");

    robot_state::RobotState ks = *robot_state;
    std::vector<double> positions(num_all_joints);
    double dt = trajectory->getDiscretization();
    // TODO:
    int num_return_points = joint_trajectory->getNumPoints();
    for (std::size_t i = 0; i < num_return_points; ++i)
    {
        for (std::size_t j = 0; j < num_all_joints; j++)
        {
            positions[j] = (*joint_trajectory)(i, j);
        }

        ks.setVariablePositions(&positions[0]);
        // TODO: copy vel/acc
        ks.update();

        response_trajectory->addSuffixWayPoint(ks, dt);
    }

    moveit_msgs::RobotTrajectory trajectory_msg;
    response_trajectory->getRobotTrajectoryMsg(trajectory_msg);

    display_trajectory.trajectory.push_back(trajectory_msg);

    ros::NodeHandle node_handle;
    static ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/itomp_planner/display_planned_path", 1, true);
    display_publisher.publish(display_trajectory);
}
Ejemplo n.º 11
0
IGL_INLINE void igl::signed_distance(
  const Eigen::MatrixXd & P,
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & F,
  const SignedDistanceType sign_type,
  Eigen::VectorXd & S,
  Eigen::VectorXi & I,
  Eigen::MatrixXd & C,
  Eigen::MatrixXd & N)
{
  using namespace Eigen;
  using namespace std;
  assert(V.cols() == 3 && "V should have 3d positions");
  assert(P.cols() == 3 && "P should have 3d positions");
  // Only unsigned distance is supported for non-triangles
  if(sign_type != SIGNED_DISTANCE_TYPE_UNSIGNED)
  {
    assert(F.cols() == 3 && "F should have triangles");
  }

  // Prepare distance computation
  AABB<MatrixXd,3> tree;
  tree.init(V,F);

  Eigen::MatrixXd FN,VN,EN;
  Eigen::MatrixXi E;
  Eigen::VectorXi EMAP;
  WindingNumberAABB<Eigen::Vector3d> hier;
  switch(sign_type)
  {
    default:
      assert(false && "Unknown SignedDistanceType");
    case SIGNED_DISTANCE_TYPE_UNSIGNED:
      // do nothing
      break;
    case SIGNED_DISTANCE_TYPE_DEFAULT:
    case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
      hier.set_mesh(V,F);
      hier.grow();
      break;
    case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
      // "Signed Distance Computation Using the Angle Weighted Pseudonormal"
      // [Bærentzen & Aanæs 2005]
      per_face_normals(V,F,FN);
      per_vertex_normals(V,F,PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE,FN,VN);
      per_edge_normals(
        V,F,PER_EDGE_NORMALS_WEIGHTING_TYPE_UNIFORM,FN,EN,E,EMAP);
      N.resize(P.rows(),3);
      break;
  }

  S.resize(P.rows(),1);
  I.resize(P.rows(),1);
  C.resize(P.rows(),3);
  for(int p = 0;p<P.rows();p++)
  {
    const RowVector3d q = P.row(p);
    double s,sqrd;
    RowVector3d c;
    int i=-1;
    switch(sign_type)
    {
      default:
        assert(false && "Unknown SignedDistanceType");
      case SIGNED_DISTANCE_TYPE_UNSIGNED:
        s = 1.;
        sqrd = tree.squared_distance(V,F,q,i,c);
        break;
      case SIGNED_DISTANCE_TYPE_DEFAULT:
      case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
        signed_distance_winding_number(tree,V,F,hier,q,s,sqrd,i,c);
        break;
      case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
      {
        Eigen::RowVector3d n;
        signed_distance_pseudonormal(tree,V,F,FN,VN,EN,EMAP,q,s,sqrd,i,c,n);
        N.row(p) = n;
        break;
      }
    }
    I(p) = i;
    S(p) = s*sqrt(sqrd);
    C.row(p) = c;
  }
}
Ejemplo n.º 12
0
void display()
{
  using namespace igl;
  using namespace std;
  using namespace Eigen;
  const float back[4] = {30.0/255.0,30.0/255.0,50.0/255.0,0};
  glClearColor(back[0],back[1],back[2],0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  if(is_animating)
  {
    double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION;
    if(t > 1)
    {
      t = 1;
      is_animating = false;
    }
    Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized();
    auto & camera = s.camera;
    camera.orbit(q.conjugate());
  }

  glEnable(GL_DEPTH_TEST);
  glEnable(GL_NORMALIZE);
  lights();
  push_scene();

  // Draw a nice floor
  glEnable(GL_DEPTH_TEST);
  glPushMatrix();
  const double floor_offset =
    -2./bbd*(V.col(1).maxCoeff()-Vmid(1));
  glTranslated(0,floor_offset,0);
  const float GREY[4] = {0.5,0.5,0.6,1.0};
  const float DARK_GREY[4] = {0.2,0.2,0.3,1.0};
  glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
  draw_floor(GREY,DARK_GREY);
  glPopMatrix();

  push_object();

  // Set material properties
  glDisable(GL_COLOR_MATERIAL);
  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT,  SILVER_AMBIENT);
  glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE,  SILVER_DIFFUSE  );
  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, SILVER_SPECULAR);
  glMaterialf (GL_FRONT_AND_BACK, GL_SHININESS, 128);

  typedef std::vector<
    Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> >
    RotationList;
  RotationList dQ(BE.rows(),Quaterniond::Identity()),vQ;
  vector<Vector3d> vT;
  Matrix3d A = Matrix3d::Identity();
  for(int e = 0;e<BE.rows();e++)
  {
    dQ[e] = AngleAxisd((sin(get_seconds()+e))*0.06*PI,A.col(e%3));
  }
  forward_kinematics(C,BE,P,dQ,vQ,vT);
  const int dim = C.cols();
  MatrixXd T(BE.rows()*(dim+1),dim);
  for(int e = 0;e<BE.rows();e++)
  {
    Affine3d a = Affine3d::Identity();
    a.translate(vT[e]);
    a.rotate(vQ[e]);
    T.block(e*(dim+1),0,dim+1,dim) =
      a.matrix().transpose().block(0,0,dim+1,dim);
  }

  if(wireframe)
  {
    glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
  }
  glLineWidth(1.0);
  MatrixXd U = M*T;
  per_face_normals(U,F,N);
  draw_mesh(U,F,N);
  glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);

  if(skeleton_on_top)
  {
    glDisable(GL_DEPTH_TEST);
  }

  switch(skel_style)
  {
    default:
    case SKEL_STYLE_TYPE_3D:
      draw_skeleton_3d(C,BE,T,MAYA_VIOLET,bbd*0.5);
      break;
    case SKEL_STYLE_TYPE_VECTOR_GRAPHICS:
      draw_skeleton_vector_graphics(C,BE,T);
      break;
  }

  pop_object();

  pop_scene();

  report_gl_error();

  TwDraw();
  glutSwapBuffers();
  glutPostRedisplay();
}
Ejemplo n.º 13
0
 inline vcl_size_t size2(Eigen::MatrixXd const & m) { return m.cols(); }
Ejemplo n.º 14
0
 inline vcl_size_t size1(Eigen::MatrixXd const & m) { return static_cast<vcl_size_t>(m.rows()); }
Eigen::MatrixXd ComputeP_NormalizedDLT(const Point2DVector& points2D, const Point3DVector& points3D)
{
  unsigned int numberOfPoints = points2D.size();
  if(points3D.size() != numberOfPoints)
    {
    std::stringstream ss;
    ss << "ComputeP_NormalizedDLT: The number of 2D points (" << points2D.size()
       << ") must match the number of 3D points (" << points3D.size() << ")!" << std::endl;
    throw std::runtime_error(ss.str());
    }

//   std::cout << "ComputeP_NormalizedDLT: 2D points: " << std::endl;
//   for(Point2DVector::const_iterator iter = points2D.begin(); iter != points2D.end(); ++iter)
//   {
//     Point2DVector::value_type p = *iter;
//     std::cout << p[0] << " " << p[1] << std::endl;
//   }

//   std::cout << "ComputeP_NormalizedDLT: 3D points: " << std::endl;
//   for(Point3DVector::const_iterator iter = points3D.begin(); iter != points3D.end(); ++iter)
//   {
//     Point3DVector::value_type p = *iter;
//     std::cout << p[0] << " " << p[1] << " " << p[2] << std::endl;
//   }
  
  Eigen::MatrixXd similarityTransform2D = ComputeNormalizationTransform<Eigen::Vector2d>(points2D);
  Eigen::MatrixXd similarityTransform3D = ComputeNormalizationTransform<Eigen::Vector3d>(points3D);

//   std::cout << "Computed similarity transforms:" << std::endl;
//   std::cout << "similarityTransform2D: " << similarityTransform2D << std::endl;
//   std::cout << "similarityTransform3D: " << similarityTransform3D << std::endl;

  // The (, Eigen::VectorXd()) below are only required when using gnu++0x, it seems to be a bug in Eigen
  Point2DVector transformed2DPoints(numberOfPoints, Eigen::Vector2d());
  Point3DVector transformed3DPoints(numberOfPoints, Eigen::Vector3d());

  for(unsigned int i = 0; i < numberOfPoints; ++i)
    {
    Eigen::VectorXd point2Dhomogeneous = points2D[i].homogeneous();
    Eigen::VectorXd point2Dtransformed = similarityTransform2D * point2Dhomogeneous;
    transformed2DPoints[i] = point2Dtransformed.hnormalized();

    Eigen::VectorXd point3Dhomogeneous = points3D[i].homogeneous();
    Eigen::VectorXd point3Dtransformed = similarityTransform3D * point3Dhomogeneous;
    transformed3DPoints[i] = point3Dtransformed.hnormalized();
  
    //transformed2DPoints[i] = (similarityTransform2D * points2D[i].homogeneous()).hnormalized();
    //transformed3DPoints[i] = (similarityTransform3D * points3D[i].homogeneous()).hnormalized();
    }

  // std::cout << "Transformed points." << std::endl;
  
  // Compute the Camera Projection Matrix

  Eigen::MatrixXd A(2*numberOfPoints,12);
  for(unsigned int i = 0; i < numberOfPoints; ++i)
    {
    // First row/equation from the ith correspondence
    unsigned int row = 2*i;
    A(row, 0) = 0;
    A(row, 1) = 0;
    A(row, 2) = 0;
    A(row, 3) = 0;
    A(row, 4) = transformed3DPoints[i](0);
    A(row, 5) = transformed3DPoints[i](1);
    A(row, 6) = transformed3DPoints[i](2);
    A(row, 7) = 1;
    A(row, 8) = -transformed2DPoints[i](1) * transformed3DPoints[i](0);
    A(row, 9) = -transformed2DPoints[i](1) * transformed3DPoints[i](1);
    A(row, 10) = -transformed2DPoints[i](1) * transformed3DPoints[i](2);
    A(row, 11) = -transformed2DPoints[i](1);

    // Second row/equation from the ith correspondence
    row = 2*i+1;
    A(row, 0) = transformed3DPoints[i](0);
    A(row, 1) = transformed3DPoints[i](1);
    A(row, 2) = transformed3DPoints[i](2);
    A(row, 3) = 1;
    A(row, 4) = 0;
    A(row, 5) = 0;
    A(row, 6) = 0;
    A(row, 7) = 0;
    A(row, 8) = -transformed2DPoints[i](0) * transformed3DPoints[i](0);
    A(row, 9) = -transformed2DPoints[i](0) * transformed3DPoints[i](1);
    A(row, 10) = -transformed2DPoints[i](0) * transformed3DPoints[i](2);
    A(row, 11) = -transformed2DPoints[i](0);
    }

  // std::cout << "A: " << A << std::endl;
  
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);

  Eigen::MatrixXd V = svd.matrixV();
  Eigen::MatrixXd lastColumnOfV = V.col(11);

  Eigen::MatrixXd P = Reshape(lastColumnOfV, 3, 4);
    
  // Denormalization
  P = similarityTransform2D.inverse()*P*similarityTransform3D; // 3x3 * 3x4 * 4x4 = 4x4

  return P;
}
Ejemplo n.º 16
0
/**
 * Generates a bathymetryGrid by reading data from a local file.  Results are
 * dumped into topographyGrid.
 * @param topographyGrid A pointer to a zero-initialized Grid of size
 *      numRows x numCols.
 * @param inputFile The relative path to the topography file you wish to use.
 * @param inputFileType Use 'netcdf' if your file is in netcdf format.  Use
 *      'asc' if the file is a matrix of ascii values in the GIS asc format.
 * @param startRow The row to start reading data from.
 * @param startCol The col to start reading data from.
 * @param numRows The desired number of rows of data to read.
 * @param numCols The desired number of cols of data to read.
 * @param seriesName If inputFileType was set to 'netcdf', this should be set
 *      to the name of the series you wish to use.
 * @param timestamp A timestamp value used for error reporting.
 */
void getBathy(Grid* topographyGrid, std::string inputFile,
              std::string inputFileType, size_t startRow, size_t startCol,
              size_t numRows, size_t numCols, std::string seriesName,
              std::string timestamp) {
    // This will be the netCDF ID for the file and data variable.
    Eigen::MatrixXd temp;
    int ncid, varid, retval = -100;

    // NetCDF
    if (inputFileType.compare("netcdf") == 0) {
        // Data will be read in column major, so set up a matrix of inverse
        //    size to recieve it.
        temp.resize(numCols, numRows);
        // Open the file. NC_NOWRITE tells netCDF we want read-only access to
        //    the file.
        if ((retval = nc_open(inputFile.c_str(), NC_NOWRITE, &ncid))) {
            printError("ERROR: Cant open NetCDF File. Invalid inputFile path.",
                        retval, timestamp);
        }

        // Get the varid of the data variable, based on its name.
        if ((retval = nc_inq_varid(ncid, seriesName.c_str(), &varid))) {
            printError("ERROR: Can't access variable id.  Invalid seriesName.",
                        retval, timestamp);
        }
        // Read the data.
        try {
            // for whatever reason, this is in column, row order.
            static size_t start[] = {startRow, startCol};
            static size_t range[] = {numRows, numCols};
            retval = nc_get_vara_double(ncid, varid, start, range,
                                        temp.data());
            // TODO(Greg) Figure out a way to read data in row wise to avoid
            //             this transposition.
            topographyGrid->data.block(border, border, numRows, numCols) =
                            temp.transpose().block(0, 0, numRows, numCols);
        }
        catch (int i) {
            printError("ERROR: Error reading data.  Invalid file format.",
                        retval, timestamp);
        }
        // Close the file, freeing all resources.
        if ((retval = nc_close(ncid))) {
            printError("ERROR: Error closing the file.", retval, timestamp);
        }
    } else if (inputFileType.compare("asc") == 0) {
        // ASC
        temp.resize(numRows, numCols);
        std::ifstream input(inputFile);
        int null = 0;
        size_t numHeaderLines = 5,
               rowsLine = 1,
               colsLine = 0,
               nullLine = 5,
               cursor = 0,
               rows = 0,
               cols = 0,
               startRowIndex = startRow+numHeaderLines,
               endRowIndex = startRowIndex+numRows,
               i = 0,
               j = 0;
        std::string line = "";
        std::vector<std::string> vLine;
        if (input.is_open()) {
            for (cursor = 0; cursor < endRowIndex; cursor ++) {
                getline(input, line);

                if (cursor <= numHeaderLines) {
                    // Get the number of columns in the file
                    if (cursor == colsLine) {
                        vLine = split(&line, ' ');
                        if (!silent) {
                            std::cout << "\nAvailable Cols: " <<
                                         vLine[vLine.size()-1];
                        }
                        cols = stoi(vLine[vLine.size() - 1]);

                        if (cols < startCol + numCols) {
                            std::cout << "\nERROR, requested bathymetry " <<
                                    " grid column coordinates are out of" <<
                                    " bounds.\n";
                            exit(1);
                        }
                    } else if (cursor == rowsLine) {
                        // Get the number of rows in the file.
                        vLine = split(&line, ' ');
                        if (!silent) {
                        std::cout << "Available Rows:" <<
                                     vLine[vLine.size() - 1];
                        }
                        rows = stoi(vLine[vLine.size() - 1]);

                        if (rows < startRow + numRows) {
                            std::cout << "\nERROR, requested bathymetry" <<
                                    " grid row coordinates are out of" <<
                                    " bounds.\n";
                            exit(1);
                        }
                    } else if (cursor == nullLine) {
                        // Get the null value substitute
                        vLine = split(&line, ' ');
                        if (debug) {
                            std::cout << "Null values =" <<
                                         vLine[vLine.size() - 1] << "\n";
                        }
                        null = stoi(vLine[vLine.size() - 1]);
                    }
                } else if (cursor >= startRowIndex) {
                    vLine = split(&line, ' ');
                    for (i = startCol; i < startCol + numCols; i ++) {
                        // std::cout<<"accessing temp(" <<
                        // cursor-startRowIndex-1 << "," <<i-startCol<<")\n";
                        // std::cout<<"Cursor:"<<cursor<<"   SRI:" <<
                        // startRowIndex << "\n";
                        temp(cursor - startRowIndex, i - startCol) =
                                stod(vLine[i]);
                    }
                }
            }

            input.close();

            for (i = 0; i < numRows; i ++) {
                for (j = 0; j < numCols; j ++) {
                    if (temp(i, j) == null) {
                        temp(i, j) = 0;
                    }
                }
            }
            topographyGrid->data.block(border, border, numRows, numCols) =
                            temp.block(0, 0, numRows, numCols);
        } else {
            std::cout << "\nUnable to open bathymetry file: \"" << inputFile <<
                         "\"\n";
            exit(0);
        }
    } else {
        // Invalid filetype
        std::cout << "Bathymetry file type not supported.  Simulating" <<
                     "Bathymetry.\n";
        simulatetopographyGrid(topographyGrid, static_cast<int>(numRows),
                               static_cast<int>(numCols));
    }
    topographyGrid->clearNA();
    topographyGrid->data =
            topographyGrid->data.unaryExpr(std::ptr_fun(validateDepth));
    if (acousticParams["debug"] == "1") {
        // topographyGrid->printData();
        std::cout << "startx " << startCol << "\nXDist: "<< numCols <<
                  "\nstartY:  "<< startRow << "\nYDist: " << numRows << "\n";
        std::cout << "inputFileType: " << inputFileType << "\ninputFile: " <<
                  inputFile << "\nseriesName: " << seriesName << "\n";
        std::cout << "retval: " << retval << "\n" << "ncid: " << ncid << "\n\n";
    }
}
Ejemplo n.º 17
0
void display()
{
  using namespace Eigen;
  using namespace igl;
  using namespace std;
  glClearColor(back[0],back[1],back[2],0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  // All smooth points
  glEnable( GL_POINT_SMOOTH );

  lights();
  push_scene();
  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);
  glEnable(GL_NORMALIZE);
  glEnable(GL_COLOR_MATERIAL);
  glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
  push_object();

  if(trackball_on)
  {
    // Draw a "laser" line
    glLineWidth(3.0);
    glDisable(GL_LIGHTING);
    glEnable(GL_DEPTH_TEST);
    glBegin(GL_LINES);
    glColor3f(1,0,0);
    glVertex3fv(s.data());
    glColor3f(1,0,0);
    glVertex3fv(d.data());
    glEnd();

    // Draw the start and end points used for ray
    glPointSize(10.0);
    glBegin(GL_POINTS);
    glColor3f(1,0,0);
    glVertex3fv(s.data());
    glColor3f(0,0,1);
    glVertex3fv(d.data());
    glEnd();
  }

  // Draw the model
  glEnable(GL_LIGHTING);
  draw_mesh(V,F,N,C);

  // Draw all hits
  glBegin(GL_POINTS);
  glColor3f(0,0.2,0.2);
  for(vector<igl::embree::Hit>::iterator hit = hits.begin();
      hit != hits.end();
      hit++)
  {
    const double w0 = (1.0-hit->u-hit->v);
    const double w1 = hit->u;
    const double w2 = hit->v;
    VectorXd hitP =
      w0 * V.row(F(hit->id,0)) +
      w1 * V.row(F(hit->id,1)) +
      w2 * V.row(F(hit->id,2));
    glVertex3dv(hitP.data());
  }
  glEnd();

  pop_object();

  // Draw a nice floor
  glPushMatrix();
  glEnable(GL_LIGHTING);
  glTranslated(0,-1,0);
  draw_floor();
  glPopMatrix();

  // draw a transparent "projection screen" show model at time of hit (aka
  // mouse down)
  push_object();
  if(trackball_on)
  {
    glColor4f(0,0,0,1.0);
    glPointSize(10.0);
    glBegin(GL_POINTS);
    glVertex3fv(SW.data());
    glVertex3fv(SE.data());
    glVertex3fv(NE.data());
    glVertex3fv(NW.data());
    glEnd();

    glDisable(GL_LIGHTING);
    glEnable(GL_TEXTURE_2D);
    glBindTexture(GL_TEXTURE_2D, tex_id);
    glColor4f(1,1,1,0.7);
    glBegin(GL_QUADS);
    glTexCoord2d(0,0);
    glVertex3fv(SW.data());
    glTexCoord2d(1,0);
    glVertex3fv(SE.data());
    glTexCoord2d(1,1);
    glVertex3fv(NE.data());
    glTexCoord2d(0,1);
    glVertex3fv(NW.data());
    glEnd();
    glBindTexture(GL_TEXTURE_2D, 0);
    glDisable(GL_TEXTURE_2D);
  }
  pop_object();
  pop_scene();

  // Draw a faint point over mouse
  if(!trackball_on)
  {
    glDisable(GL_LIGHTING);
    glDisable(GL_COLOR_MATERIAL);
    glDisable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    glColor4f(1.0,0.3,0.3,0.6);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluOrtho2D(0,width,0,height);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    glPointSize(20.0);
    glBegin(GL_POINTS);
    glVertex2fv(win_s.data());
    glEnd();
  }
  report_gl_error();

  glutSwapBuffers();
  glutPostRedisplay();
}
Ejemplo n.º 18
0
/**
 * Generates an artificial topographyGrid of size numRows x numCols if no
 * topographic data is available.  Results are dumped into topographyGrid.
 * @param topographyGrid A pointer to a zero-initialized Grid of size
 * numRows x numCols.
 * @param numRows The desired number of non-border rows in the resulting matrix.
 * @param numCols The desired number of non-border cols in the resulting matrix.
 */
void simulatetopographyGrid(Grid* topographyGrid, int numRows, int numCols) {
    Eigen::VectorXd refx = refx.LinSpaced(numCols, -2*M_PI, 2*M_PI);
    Eigen::VectorXd refy = refx.LinSpaced(numRows, -2*M_PI, 2*M_PI);
    Eigen::MatrixXd X = refx.replicate(1, numRows);
    X.transposeInPlace();
    Eigen::MatrixXd Y = refy.replicate(1, numCols);

    // Eigen can only deal with two matrices at a time,
    // so split the computation:
    // topographyGrid = sin(X) * sin(Y) * abs(X) * abs(Y) -pi
    Eigen::MatrixXd absXY = X.cwiseAbs().cwiseProduct(Y.cwiseAbs());
    Eigen::MatrixXd sins = X.array().sin().cwiseProduct(Y.array().sin());
    Eigen::MatrixXd temp;
    temp.resize(numRows, numCols);
    temp = absXY.cwiseProduct(sins);

    // All this work to create a matrix of pi...
    Eigen::MatrixXd pi;
    pi.resize(numRows, numCols);
    pi.setConstant(M_PI);
    temp = temp - pi;
    // And the final result.
    topographyGrid->data.block(border, border, numRows, numCols) =
                              temp.block(0, 0, numRows, numCols);
    // Ignore positive values.
    topographyGrid->data =
            topographyGrid->data.unaryExpr(std::ptr_fun(validateDepth));
    topographyGrid->clearNA();
}
Ejemplo n.º 19
0
void CDKF::measurementUpdate(const ros::Time& prev_stamp,
                             const ros::Time& current_stamp,
                             const double image_angular_velocity,
                             const IMUList& imu_rotations,
                             const bool calc_offset) {
  // convert tracked points to measurement
  Eigen::VectorXd real_measurement(kMeasurementSize);
  accessM(real_measurement, MEASURED_TIMESTAMP).array() =
      (current_stamp - zero_timestamp_).toSec();

  accessM(real_measurement, ANGULAR_VELOCITY).array() = image_angular_velocity;

  if (verbose_) {
    ROS_INFO_STREAM("Measured Values: \n" << real_measurement.transpose());
  }

  // create sigma points
  MeasurementSigmaPoints sigma_points(
      state_, cov_, measurement_noise_sd_, CDKF::stateToMeasurementEstimate,
      imu_rotations, zero_timestamp_, calc_offset);

  // get mean and cov
  Eigen::VectorXd predicted_measurement;
  sigma_points.calcEstimatedMean(&predicted_measurement);
  Eigen::MatrixXd innovation;
  sigma_points.calcEstimatedCov(&innovation);

  if (verbose_) {
    ROS_INFO_STREAM("Predicted Measurements: \n"
                    << predicted_measurement.transpose());
  }

  // calc mah distance
  Eigen::VectorXd diff = real_measurement - predicted_measurement;
  double mah_dist = std::sqrt(diff.transpose() * innovation.inverse() * diff);
  if (mah_dist > mah_threshold_) {
    ROS_WARN("Outlier detected, measurement rejected");
    return;
  }

  Eigen::MatrixXd cross_cov;
  sigma_points.calcEstimatedCrossCov(&cross_cov);

  Eigen::MatrixXd gain = cross_cov * innovation.inverse();

  const Eigen::VectorXd state_diff =
      gain * (real_measurement - predicted_measurement);

  state_ += state_diff;

  cov_ -= gain * innovation * gain.transpose();

  if (verbose_) {
    ROS_INFO_STREAM("Updated State: \n" << state_.transpose());
    ROS_INFO_STREAM("Updated Cov: \n" << cov_);
  }

  // guard against precision issues
  constexpr double kMaxTime = 10000.0;
  if (accessS(state_, STATE_TIMESTAMP)[0] > kMaxTime) {
    rezeroTimestamps(current_stamp);
  }
}
Ejemplo n.º 20
0
void display()
{
  if(sorted_F.size() != F.size())
  {
    mouse(0,1,-1,-1);
  }
  //using namespace Eigen;
  using namespace igl;
  using namespace std;
  glClearColor(BG[0],BG[1],BG[2],0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  // All smooth points
  glEnable( GL_POINT_SMOOTH );

  lights();
  push_scene();
  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);
  glEnable(GL_NORMALIZE);

  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  //glEnable(GL_CULL_FACE);
  //glCullFace(GL_BACK);

  // Draw a nice floor
  push_object();
  glEnable(GL_LIGHTING);
  glTranslated(0,V.col(1).minCoeff(),0);
  glScaled(2*bbd,2*bbd,2*bbd);
  glTranslated(0,-1000*FLOAT_EPS,0);
  glEnable(GL_CULL_FACE);
  glCullFace(GL_BACK);
  igl::opengl2::draw_floor();
  pop_object();

  glDisable(GL_CULL_FACE);
  if(trackball_on)
  {
    glEnable(GL_COLOR_MATERIAL);
    glDisable(GL_LIGHTING);
  }else
  {
    float front[4];
    copy(GOLD_DIFFUSE,GOLD_DIFFUSE+3,front);
    float back[4];
    //copy(FAST_GREEN_DIFFUSE,FAST_GREEN_DIFFUSE+3,back);
    copy(GOLD_DIFFUSE,GOLD_DIFFUSE+3,back);
    float amb[4];
    copy(SILVER_AMBIENT,SILVER_AMBIENT+3,amb);
    float spec[4];
    copy(SILVER_SPECULAR,SILVER_SPECULAR+3,spec);
    front[3] = back[3] = amb[3] = spec[3] = alpha;
    // Set material properties
    glDisable(GL_COLOR_MATERIAL);
    glMaterialfv(GL_FRONT, GL_AMBIENT,  amb);
    glMaterialfv(GL_FRONT, GL_DIFFUSE,  front);
    glMaterialfv(GL_FRONT, GL_SPECULAR, spec);
    glMaterialf (GL_FRONT, GL_SHININESS, 128);
    glMaterialfv(GL_BACK, GL_AMBIENT,  amb);
    glMaterialfv(GL_BACK, GL_DIFFUSE,  back);
    glMaterialfv(GL_BACK, GL_SPECULAR, spec);
    glMaterialf (GL_BACK, GL_SHININESS, 128);
    glEnable(GL_LIGHTING);
  }

  push_object();
  // Draw the model
  igl::opengl2::draw_mesh(V,sorted_F,sorted_N,C);
  pop_object();

  pop_scene();

  igl::opengl::report_gl_error();
  glutSwapBuffers();
  glutPostRedisplay();
}
Ejemplo n.º 21
0
TEST_F(ExoticaTaskTest, DTT) //!< Derived-task Jacobian Tests: will contain tests to check whether the jacobian is correctly computed through finite-differences method
{
    boost::shared_ptr<exotica::TaskDefinition> task_ptr;
    for (int i = 0; i < registered_types_.size(); i++) //!< Iterate through the registered tasks
    {
        std::string xml_path;
        tinyxml2::XMLDocument document;
        boost::shared_ptr<tinyxml2::XMLHandle> element_ptr;
        if (exotica::TestRegistrar::Instance()->findXML(registered_types_[i],
                xml_path))  //!< If it is wished to be tested...
        {
            if (document.LoadFile((resource_path_ + xml_path).c_str())
                    != tinyxml2::XML_NO_ERROR) //!< Attempt to load file
            {
                ADD_FAILURE()<< " : Could not Load initialiser for " << registered_types_[i] << " (file: "<<resource_path_ + xml_path <<")."; //!< Have to use this method to add failure since I do not want to continue for this object but do not wish to abort for all the others...
                continue;//!< Go to next object
            }
            //!< Initialise
            if (!(task_ptr = exotica::TaskCreator::Instance()->createObject(registered_types_[i], params_)))//!< If we could not create
            {
                ADD_FAILURE() << " : Could not create object of type " << registered_types_[i]; //!< Have to use this method to add failure since I do not want to continue for this object but do not wish to abort for all the others...
                continue;//!< Go to next object
            }
            tinyxml2::XMLHandle xml_handle(document.RootElement()); //!< Get a handle to the root element
            if (!task_ptr->initBase(xml_handle))
            {
                ADD_FAILURE() << " : XML Initialiser is malformed for " << registered_types_[i];
                continue;
            }

            //!< Now our testing stuff...
            element_ptr.reset(new tinyxml2::XMLHandle(xml_handle.FirstChildElement("TestPoint")));
            if (!element_ptr)
            {
                ADD_FAILURE() << " : XML Tester is malformed for " << registered_types_[i];
                continue;
            }
            int j=0;
            while (element_ptr->ToElement()) //!< Iterate through all possible test cases
            {
                Eigen::VectorXd conf_space; //!< Vector containing the configuration point for testing
                Eigen::VectorXd task_space;//!< Vector with the task-space co-ordinate of the forward map
                Eigen::VectorXd phi;//!< The computed phi
                Eigen::MatrixXd jacobian;//!< The Jacobian computation
                if (!element_ptr->FirstChildElement("ConfSpace").ToElement())//!< If inexistent
                {
                    ADD_FAILURE() << " : Missing Config for " << registered_types_[i] << " @ iteration " << j;
                    element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
                    j++;
                    continue; //!< With the inner while loop...
                }
                if (!exotica::getVector(*(element_ptr->FirstChildElement("ConfSpace").ToElement()), conf_space))
                {
                    ADD_FAILURE() << " : Could not parse Config Vector for " << registered_types_[i] << " @ iteration " << j;
                    element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
                    j++;
                    continue; //!< With the inner while loop...
                }
                if (!element_ptr->FirstChildElement("TaskSpace").ToElement()) //!< If inexistent
                {
                    ADD_FAILURE() << " : Missing Task Space point for " << registered_types_[i] << " @ iteration " << j;
                    element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
                    j++;
                    continue; //!< With the inner while loop...
                }
                if (!exotica::getVector(*(element_ptr->FirstChildElement("TaskSpace").ToElement()), task_space))
                {
                    ADD_FAILURE() << " : Could not parse task vector for " << registered_types_[i] << " @ iteration " << j;
                    element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
                    j++;
                    continue; //!< With the inner while loop...
                }

                //!< First test forward mapping
                if (!task_ptr->updateTask(conf_space, 0))
                {
                    ADD_FAILURE() << " for " << registered_types_[i] << " @ iteration " << j;
                    element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
                    j++;
                    continue;
                }
                phi = task_ptr->getPhi(0);
                jacobian = task_ptr->getJacobian(0);
                if (phi.size() != task_space.size())
                {
                    ADD_FAILURE() << " Task-size mismatch for " << registered_types_[i] << " @ iteration " << j;
                    element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
                    j++;
                    continue;
                }
                EXPECT_TRUE(compareVectors(phi, task_space, TOLERANCE)) << " : Phi mismatch for " << registered_types_[i] << " @ iteration " << j;

                //!< Now the Jacobian (finite differences method)
                for (int k=0; k<conf_space.size(); k++)
                {
                    Eigen::VectorXd conf_perturb = conf_space;
                    conf_perturb(k) += EPSILON;
                    if (!task_ptr->updateTask(conf_perturb, 0))
                    {
                        ADD_FAILURE() << " for " << registered_types_[i] << " @ iteration " << j << " in config-dimension " << k;
                        continue;
                    }
                    if (task_ptr->getPhi(0).size() != phi.size())
                    {
                        ADD_FAILURE() << " for " << registered_types_[i] << " @ iteration " << j << " in config-dimension " << k;
                        continue;
                    }
                    Eigen::VectorXd task_diff = (task_ptr->getPhi(0) - phi)/EPSILON; //!< Final - initial
                    Eigen::VectorXd jac_column = jacobian.col(k);//!< Get the desired column (for the current joint)
                    EXPECT_TRUE(compareVectors(task_diff, jac_column, TOLERANCE)) << " Incorrect for " << registered_types_[i] << " @ iteration " << j << " in config-dimension " << k << task_diff << "\n" << jac_column;
                }
                element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
                j++;
            } //!< Iteration through j
        } //check on testing
    } //!< Iteration through i
}
Ejemplo n.º 22
0
// computes the optimal rigid body transformation given a set of points
void PoseTracker::computeOptimalRigidTransformation(Eigen::MatrixXd startP, Eigen::MatrixXd finalP)
{	
	Eigen::Matrix4d transf;
	
	if (startP.rows()!=finalP.rows())
	{	
		ROS_ERROR("We need that the rows be the same at the beggining");
		exit(1);
	}

	Eigen::RowVector3d centroid_startP = Eigen::RowVector3d::Zero(); 
	Eigen::RowVector3d centroid_finalP = Eigen::RowVector3d::Zero(); //= mean(B);
	Eigen::Matrix3d H = Eigen::Matrix3d::Zero();

	//calculate the mean
	for (int i=0;i<startP.rows();i++)
	{	
		centroid_startP = centroid_startP+startP.row(i);
		centroid_finalP = centroid_finalP+finalP.row(i);
	}
	
	centroid_startP = centroid_startP/startP.rows();
	centroid_finalP = centroid_finalP/startP.rows();

	for (int i=0;i<startP.rows();i++)
		H = H + (startP.row(i)-centroid_startP).transpose()*(finalP.row(i)-centroid_finalP);

   	Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
   
    Eigen::MatrixXd U = svd.matrixU();
   	Eigen::MatrixXd V = svd.matrixV();
  
    if (V.determinant()<0)
   		V.col(2)=-V.col(2)*(-1);

	Eigen::MatrixXd R = V*U.transpose();

	Eigen::Matrix4d C_A = Eigen::Matrix4d::Identity();
	Eigen::Matrix4d C_B = Eigen::Matrix4d::Identity();
	Eigen::Matrix4d R_new = Eigen::Matrix4d::Identity();
			
	C_A.block<3,1>(0,3) = -centroid_startP.transpose();
	R_new.block<3,3>(0,0) = R;
	
	C_B.block<3,1>(0,3) = centroid_finalP.transpose();


	transf = C_B * R_new * C_A;

	Eigen::Quaterniond mat_rot(transf.block<3,3>(0,0));

	Eigen::Vector3d trasl = transf.block<3,1>(0,3).transpose();

	transfParameters_ << trasl(0), trasl(1), trasl(2), mat_rot.x(), mat_rot.y(), mat_rot.z(), mat_rot.w();

}
Ejemplo n.º 23
0
void display()
{
  using namespace igl;
  using namespace std;
  using namespace Eigen;
  const float back[4] = {0.75, 0.75, 0.75,0};
  glClearColor(back[0],back[1],back[2],0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  static bool first = true;
  if(first)
  {
    sort();
    first = false;
  }

  if(is_animating)
  {
    double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION;
    if(t > 1)
    {
      t = 1;
      is_animating = false;
    }
    Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized();
    camera.orbit(q.conjugate());
  }

  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);
  glEnable(GL_NORMALIZE);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  lights();
  push_scene();

  // Draw a nice floor
  glEnable(GL_DEPTH_TEST);
  glPushMatrix();
  const double floor_offset =
    -2./bbd*(V.col(1).maxCoeff()-Vmid(1));
  glTranslated(0,floor_offset,0);
  const float GREY[4] = {0.5,0.5,0.6,1.0};
  const float DARK_GREY[4] = {0.2,0.2,0.3,1.0};
  glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
  glEnable(GL_CULL_FACE);
  glCullFace(GL_BACK);
  draw_floor(GREY,DARK_GREY);
  glDisable(GL_CULL_FACE);
  glPopMatrix();

  push_object();

  const auto & draw_skeleton = []()
  {
    switch(skel_style)
    {
      default:
      case SKEL_STYLE_TYPE_3D:
      {
        MatrixXf colors = MAYA_VIOLET.transpose().replicate(s.BE.rows(),1);
        for(int si=0;si<s.sel.size();si++)
        {
          for(int b=0;b<s.BE.rows();b++)
          {
            if(s.BE(b,0) == s.sel(si) || s.BE(b,1) == s.sel(si))
            {
              colors.row(b) = MAYA_SEA_GREEN;
            }
          }
        }
        draw_skeleton_3d(s.C,s.BE,MatrixXd(),colors);
        break;
      }
      case SKEL_STYLE_TYPE_VECTOR_GRAPHICS:
        draw_skeleton_vector_graphics(s.C,s.BE);
        break;
    }
  };
  
  if(!skeleton_on_top)
  {
    draw_skeleton();
  }

  // Set material properties
  glDisable(GL_COLOR_MATERIAL);
  glMaterialfv(GL_FRONT, GL_AMBIENT,
    Vector4f(GOLD_AMBIENT[0],GOLD_AMBIENT[1],GOLD_AMBIENT[2],alpha).data());
  glMaterialfv(GL_FRONT, GL_DIFFUSE,
    Vector4f(GOLD_DIFFUSE[0],GOLD_DIFFUSE[1],GOLD_DIFFUSE[2],alpha).data());
  glMaterialfv(GL_FRONT, GL_SPECULAR,
    Vector4f(GOLD_SPECULAR[0],GOLD_SPECULAR[1],GOLD_SPECULAR[2],alpha).data());
  glMaterialf (GL_FRONT, GL_SHININESS, 128);
  glMaterialfv(GL_BACK, GL_AMBIENT,
    Vector4f(SILVER_AMBIENT[0],SILVER_AMBIENT[1],SILVER_AMBIENT[2],alpha).data());
  glMaterialfv(GL_BACK, GL_DIFFUSE,
    Vector4f(FAST_GREEN_DIFFUSE[0],FAST_GREEN_DIFFUSE[1],FAST_GREEN_DIFFUSE[2],alpha).data());
  glMaterialfv(GL_BACK, GL_SPECULAR,
    Vector4f(SILVER_SPECULAR[0],SILVER_SPECULAR[1],SILVER_SPECULAR[2],alpha).data());
  glMaterialf (GL_BACK, GL_SHININESS, 128);

  if(wireframe)
  {
    glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
  }
  glLineWidth(1.0);
  draw_mesh(V,sorted_F,sorted_N);
  glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);

  if(skeleton_on_top)
  {
    glDisable(GL_DEPTH_TEST);
    draw_skeleton();
  }

  pop_object();
  pop_scene();

  report_gl_error();

  TwDraw();
  glutSwapBuffers();
  if(is_animating)
  {
    glutPostRedisplay();
  }
}
Ejemplo n.º 24
0
RcppExport SEXP halfTransform (SEXP _transform)
{
BEGIN_RCPP
    RObject transform(_transform);
    RObject result;
    if (transform.inherits("affine"))
    {
        Eigen::MatrixXd matrix = as<Eigen::MatrixXd>(_transform);
        matrix = (matrix.log() * 0.5).exp();
        result = AffineMatrix(matrix);
    }
    else
    {
        NiftiImage transformationImage(_transform);
        switch (reg_round(transformationImage->intent_p1))
        {
            case SPLINE_GRID:
            reg_getDisplacementFromDeformation(transformationImage);
            reg_tools_multiplyValueToImage(transformationImage,transformationImage,0.5f);
            reg_getDeformationFromDisplacement(transformationImage);
            break;
            
            case DEF_FIELD:
            reg_getDisplacementFromDeformation(transformationImage);
            reg_tools_multiplyValueToImage(transformationImage,transformationImage,0.5f);
            reg_getDeformationFromDisplacement(transformationImage);
            break;
            
            case DISP_FIELD:
            reg_tools_multiplyValueToImage(transformationImage,transformationImage,0.5f);
            break;
            
            case SPLINE_VEL_GRID:
            reg_getDisplacementFromDeformation(transformationImage);
            reg_tools_multiplyValueToImage(transformationImage,transformationImage,0.5f);
            reg_getDeformationFromDisplacement(transformationImage);
            --transformationImage->intent_p2;
            if (transformationImage->num_ext>1)
                --transformationImage->num_ext;
            break;
            
            case DEF_VEL_FIELD:
            reg_getDisplacementFromDeformation(transformationImage);
            reg_tools_multiplyValueToImage(transformationImage,transformationImage,0.5f);
            reg_getDeformationFromDisplacement(transformationImage);
            --transformationImage->intent_p2;
            break;
            
            case DISP_VEL_FIELD:
            reg_tools_multiplyValueToImage(transformationImage,transformationImage,0.5f);
            --transformationImage->intent_p2;
            break;
            
            default:
            throw std::runtime_error("The specified transformation image is not valid or not supported");
        }
        
        result = transformationImage.toPointer("F3D transformation");
    }
    
    result.attr("source") = transform.attr("source");
    result.attr("target") = transform.attr("target");
    
    return result;
END_RCPP
}
Ejemplo n.º 25
0
/**
 * Makes a Grid out of an existing Eigen matrix.
 * @param dat An existing eigen matrix.
 * @param newName The name of the newly created Grid.  Used when writing files.
 */
Grid::Grid(Eigen::MatrixXd dat, std::string newName) {
    rows = dat.rows();
    cols = dat.cols();
    name = newName;
    data = dat;
}
Ejemplo n.º 26
0
Eigen::MatrixXd padMatrix(Eigen::MatrixXd d, int r)
{
    using namespace Eigen;

    MatrixXd out = MatrixXd::Zero(d.rows()+2*r, d.cols()+2*r);
    out.block(r, r, d.rows(), d.cols()) = d;
    out.block(r, 0, d.rows(), r) =
        d.block(0, 0, d.rows(), r).rowwise().reverse();
    out.block(r, d.cols()+r, d.rows(), r) =
        d.block(0, d.cols()-r, d.rows(), r).rowwise().reverse();
    out.block(0, 0, r, out.cols()) =
        out.block(r, 0, r, out.cols()).colwise().reverse();
    out.block(d.rows()+r, 0, r, out.cols()) =
        out.block(out.rows()-r, 0, r, out.cols()).colwise().reverse();

    return out;
}
void propa_2d()
{
    trace.beginBlock("2d propagation");

    typedef DiscreteExteriorCalculus<2, 2, EigenLinearAlgebraBackend> Calculus;
		typedef DiscreteExteriorCalculusFactory<EigenLinearAlgebraBackend> CalculusFactory;

    {
        trace.beginBlock("solving time dependent equation");

        const Calculus::Scalar cc = 8; // px/s
        trace.info() << "cc = " << cc << endl;

        const Z2i::Domain domain(Z2i::Point(0,0), Z2i::Point(29,29));
        const Calculus calculus = CalculusFactory::createFromDigitalSet(generateDiskSet(domain), false);

        //! [time_laplace]
        const Calculus::DualIdentity0 laplace = calculus.laplace<DUAL>() + 1e-8 * calculus.identity<0, DUAL>();
        //! [time_laplace]
        trace.info() << "laplace = " << laplace << endl;

        trace.beginBlock("finding eigen pairs");
        //! [time_eigen]
        typedef Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> EigenSolverMatrix;
        const EigenSolverMatrix eigen_solver(laplace.myContainer);

        const Eigen::VectorXd eigen_values = eigen_solver.eigenvalues();
        const Eigen::MatrixXd eigen_vectors = eigen_solver.eigenvectors();
        //! [time_eigen]
        trace.info() << "eigen_values_range = " << eigen_values.minCoeff() << "/" << eigen_values.maxCoeff() << endl;
        trace.endBlock();

        //! [time_omega]
        const Eigen::VectorXd angular_frequencies = cc * eigen_values.array().sqrt();
        //! [time_omega]

        Eigen::VectorXcd initial_wave = Eigen::VectorXcd::Zero(calculus.kFormLength(0, DUAL));
        //set_initial_phase_null(calculus, initial_wave);
        set_initial_phase_dir_yy(calculus, initial_wave);

        {
            Board2D board;
            board << domain;
            board << CustomStyle("KForm", new KFormStyle2D(-1, 1));
            board << Calculus::DualForm0(calculus, initial_wave.real());
            board.saveSVG("propagation_time_wave_initial_coarse.svg");
        }

        //! [time_init_proj]
        Eigen::VectorXcd initial_projections = eigen_vectors.transpose() * initial_wave;
        //! [time_init_proj]

        // low pass
        //! [time_low_pass]
        const Calculus::Scalar lambda_cutoff = 4.5;
        const Calculus::Scalar angular_frequency_cutoff = 2*M_PI * cc / lambda_cutoff;
        int cutted = 0;
        for (int kk=0; kk<initial_projections.rows(); kk++)
        {
            const Calculus::Scalar angular_frequency = angular_frequencies(kk);
            if (angular_frequency < angular_frequency_cutoff) continue;
            initial_projections(kk) = 0;
            cutted ++;
        }
        //! [time_low_pass]
        trace.info() << "cutted = " << cutted << "/" << initial_projections.rows() << endl;

        {
            const Eigen::VectorXcd wave = eigen_vectors * initial_projections;
            Board2D board;
            board << domain;
            board << CustomStyle("KForm", new KFormStyle2D(-1, 1));
            board << Calculus::DualForm0(calculus, wave.real());
            board.saveSVG("propagation_time_wave_initial_smoothed.svg");
        }

        const int kk_max = 60;
        trace.progressBar(0,kk_max);
        for (int kk=0; kk<kk_max; kk++)
        {
            //! [time_solve_time]
            const Calculus::Scalar time = kk/20.;
            const Eigen::VectorXcd current_projections = (angular_frequencies * std::complex<double>(0,time)).array().exp() * initial_projections.array();
            const Eigen::VectorXcd current_wave = eigen_vectors * current_projections;
            //! [time_solve_time]

            std::stringstream ss;
            ss << "propagation_time_wave_solution_" << std::setfill('0') << std::setw(3) << kk << ".svg";

            Board2D board;
            board << domain;
            board << CustomStyle("KForm", new KFormStyle2D(-1, 1));
            board << Calculus::DualForm0(calculus, current_wave.real());
            board.saveSVG(ss.str().c_str());

            trace.progressBar(kk+1,kk_max);
        }
        trace.info() << endl;

        trace.endBlock();
    }

    {
        trace.beginBlock("forced oscillations");

        const Z2i::Domain domain(Z2i::Point(0,0), Z2i::Point(50,50));
        const Calculus calculus = CalculusFactory::createFromDigitalSet(generateDiskSet(domain), false);

        const Calculus::DualIdentity0 laplace = calculus.laplace<DUAL>();
        trace.info() << "laplace = " << laplace << endl;

        trace.beginBlock("finding eigen pairs");
        typedef Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> EigenSolverMatrix;
        const EigenSolverMatrix eigen_solver(laplace.myContainer);

        const Eigen::VectorXd laplace_eigen_values = eigen_solver.eigenvalues();
        const Eigen::MatrixXd laplace_eigen_vectors = eigen_solver.eigenvectors();
        trace.info() << "eigen_values_range = " << laplace_eigen_values.minCoeff() << "/" << laplace_eigen_values.maxCoeff() << endl;
        trace.endBlock();

        Calculus::DualForm0 concentration(calculus);
        {
            const Z2i::Point point(25,25);
            const Calculus::Cell cell = calculus.myKSpace.uSpel(point);
            const Calculus::Index index = calculus.getCellIndex(cell);
            concentration.myContainer(index) = 1;
        }

        {
            Board2D board;
            board << domain;
            board << concentration;
            board.saveSVG("propagation_forced_concentration.svg");
        }

        for (int ll=0; ll<6; ll++)
        {
            //! [forced_lambda]
            const Calculus::Scalar lambda = 4*20.75/(1+2*ll);
            //! [forced_lambda]
            trace.info() << "lambda = " << lambda << endl;

            //! [forced_dalembert_eigen]
            const Eigen::VectorXd dalembert_eigen_values = (laplace_eigen_values.array() - (2*M_PI/lambda)*(2*M_PI/lambda)).array().inverse();
            const Eigen::MatrixXd concentration_to_wave = laplace_eigen_vectors * dalembert_eigen_values.asDiagonal() * laplace_eigen_vectors.transpose();
            //! [forced_dalembert_eigen]

            //! [forced_wave]
            Calculus::DualForm0 wave(calculus, concentration_to_wave * concentration.myContainer);
            //! [forced_wave]
            wave.myContainer /= wave.myContainer(calculus.getCellIndex(calculus.myKSpace.uSpel(Z2i::Point(25,25))));

            {
                trace.info() << "saving samples" << endl;
                const Calculus::Properties properties = calculus.getProperties();
                {
                    std::stringstream ss;
                    ss << "propagation_forced_samples_hv_" << ll << ".dat";
                    std::ofstream handle(ss.str().c_str());
                    for (int kk=0; kk<=50; kk++)
                    {
                        const Z2i::Point point_horizontal(kk,25);
                        const Z2i::Point point_vertical(25,kk);
                        const Calculus::Scalar xx = 2 * (kk/50. - .5);
                        handle << xx << " ";
                        handle << sample_dual_0_form<Calculus>(properties, wave, point_horizontal) << " ";
                        handle << sample_dual_0_form<Calculus>(properties, wave, point_vertical) << endl;
                    }
                }

                {
                    std::stringstream ss;
                    ss << "propagation_forced_samples_diag_" << ll << ".dat";
                    std::ofstream handle(ss.str().c_str());
                    for (int kk=0; kk<=50; kk++)
                    {
                        const Z2i::Point point_diag_pos(kk,kk);
                        const Z2i::Point point_diag_neg(kk,50-kk);
                        const Calculus::Scalar xx = 2 * sqrt(2) * (kk/50. - .5);
                        handle << xx << " ";
                        handle << sample_dual_0_form<Calculus>(properties, wave, point_diag_pos) << " ";
                        handle << sample_dual_0_form<Calculus>(properties, wave, point_diag_neg) << endl;
                    }
                }
            }

            {
                std::stringstream ss;
                ss << "propagation_forced_wave_" << ll << ".svg";
                Board2D board;
                board << domain;
                board << CustomStyle("KForm", new KFormStyle2D(-.5, 1));
                board << wave;
                board.saveSVG(ss.str().c_str());
            }
        }

        trace.endBlock();
    }
    trace.endBlock();
}
	virtual void operate() {

		time_vector[0] = time_vector[1];
		time_vector[1] = this->time.getValue();
		del_time = time_vector[0] - time_vector[1];

		for (j = 0; j < mode - 1; j++) {
			for (i = 0; i < (int) DOF; i++) {
				container(j, i) = container(j + 1, i);
			}
		}
		for (i = 0; i < (int) DOF; i++) // updating the last entry
				{
			container(mode - 1, i) = this->inputSignal.getValue()[i];
		}

		if (mode == 5) // for mode 2
				{
			diff_output = (-2.0 * container.block(2, 0, 1, (int) DOF)
					+ container.block(0, 0, 1, (int) DOF)
					+ container.block(4, 0, 1, (int) DOF))
					/ (4.0 * del_time * del_time);

		}
		if (mode == 7) // for mode 3
				{
			diff_output = (-4.0 * container.block(3, 0, 1, (int) DOF)
					- (container.block(2, 0, 1, (int) DOF)
							+ container.block(4, 0, 1, (int) DOF))
					+ 2.0
							* (container.block(1, 0, 1, (int) DOF)
									+ container.block(5, 0, 1, (int) DOF))
					+ (container.block(0, 0, 1, (int) DOF)
							+ container.block(6, 0, 1, (int) DOF)))
					/ (16.0 * del_time * del_time);
		}

		if (mode == 9) // for mode 3
				{
			diff_output = (-10.0 * container.block(4, 0, 1, (int) DOF)
					- 4.0
							* (container.block(3, 0, 1, (int) DOF)
									+ container.block(5, 0, 1, (int) DOF))
					+ 4.0
							* (container.block(2, 0, 1, (int) DOF)
									+ container.block(6, 0, 1, (int) DOF))
					+ 4.0
							* (container.block(1, 0, 1, (int) DOF)
									+ container.block(7, 0, 1, (int) DOF))
					+ 1.0
							* (container.block(0, 0, 1, (int) DOF)
									+ container.block(8, 0, 1, (int) DOF)))
					/ (64.0 * del_time * del_time);
		}

		for (i = 1; i < (int) DOF; i++) {
			tmp_out_value[i] = diff_output[i];
		}

		this->outputSignalOutputValue->setData(&tmp_out_value);

	}
Ejemplo n.º 29
0
void BranchList::findBranchesInCenterline(Eigen::MatrixXd positions)
{
	positions = sortMatrix(2,positions);
	Eigen::MatrixXd positionsNotUsed = positions;

//	int minIndex;
	int index;
	int splitIndex;
	Eigen::MatrixXd::Index startIndex;
	BranchPtr branchToSplit;
    while (positionsNotUsed.cols() > 0)
	{
		if (!mBranches.empty())
		{
			double minDistance = 1000;
			for (int i = 0; i < mBranches.size(); i++)
			{
				std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distances;
				distances = dsearchn(positionsNotUsed, mBranches[i]->getPositions());
				double d = distances.second.minCoeff(&index);
				if (d < minDistance)
				{
					minDistance = d;
					branchToSplit = mBranches[i];
					startIndex = index;
					if (minDistance < 2)
						break;
				}
			}
			std::pair<Eigen::MatrixXd::Index, double> dsearchResult = dsearch(positionsNotUsed.col(startIndex) , branchToSplit->getPositions());
			splitIndex = dsearchResult.first;
		}
		else //if this is the first branch. Select the top position (Trachea).
			startIndex = positionsNotUsed.cols() - 1;

		std::pair<Eigen::MatrixXd,Eigen::MatrixXd > connectedPointsResult = findConnectedPointsInCT(startIndex , positionsNotUsed);
		Eigen::MatrixXd branchPositions = connectedPointsResult.first;
		positionsNotUsed = connectedPointsResult.second;

		if (branchPositions.cols() >= 5) //only include brances of length >= 5 points
		{
			BranchPtr newBranch = BranchPtr(new Branch());
			newBranch->setPositions(branchPositions);
			mBranches.push_back(newBranch);

			if (mBranches.size() > 1) // do not try to split another branch when the first branch is processed
			{
				if ((splitIndex + 1 >= 5) && (branchToSplit->getPositions().cols() - splitIndex - 1 >= 5))
					//do not split branch if the new branch is close to the edge of the branch
					//if the new branch is not close to one of the edges of the
					//connected existing branch: Split the existing branch
				{
					BranchPtr newBranchFromSplit = BranchPtr(new Branch());
					Eigen::MatrixXd branchToSplitPositions = branchToSplit->getPositions();
					newBranchFromSplit->setPositions(branchToSplitPositions.rightCols(branchToSplitPositions.cols() - splitIndex - 1));
					branchToSplit->setPositions(branchToSplitPositions.leftCols(splitIndex + 1));
					mBranches.push_back(newBranchFromSplit);
					newBranchFromSplit->setParentBranch(branchToSplit);
					newBranch->setParentBranch(branchToSplit);
					newBranchFromSplit->setChildBranches(branchToSplit->getChildBranches());
					branchVector branchToSplitChildren = branchToSplit->getChildBranches();
					for (int i = 0; i < branchToSplitChildren.size(); i++)
						branchToSplitChildren[i]->setParentBranch(newBranchFromSplit);
					branchToSplit->deleteChildBranches();
					branchToSplit->addChildBranch(newBranchFromSplit);
					branchToSplit->addChildBranch(newBranch);
				}
				else if (splitIndex + 1 < 5)
					 // If the new branch is close to the start of the existing
					 // branch: Connect it to the same position start as the
					 // existing branch
				{
					newBranch->setParentBranch(branchToSplit->getParentBranch());
					branchToSplit->getParentBranch()->addChildBranch(newBranch);
				}
				else if (branchToSplit->getPositions().cols() - splitIndex - 1 < 5)
					// If the new branch is close to the end of the existing
					// branch: Connect it to the end of the existing branch
				{
					newBranch->setParentBranch(branchToSplit);
					branchToSplit->addChildBranch(newBranch);
				}

			}

		}
	}
}
Ejemplo n.º 30
0
void run() {
  auto input_image = Image<float>::open (argument[0]).with_direct_io (3);

  Eigen::MatrixXd grad = DWI::get_valid_DW_scheme (input_image);

  // Want to support non-shell-like data if it's just a straight extraction
  //   of all dwis or all bzeros i.e. don't initialise the Shells class
  std::vector<size_t> volumes;
  bool bzero = get_options ("bzero").size();
  auto opt = get_options ("shell");
  if (opt.size()) {
    DWI::Shells shells (grad);
    shells.select_shells (false, false);
    for (size_t s = 0; s != shells.count(); ++s) {
      DEBUG ("Including data from shell b=" + str(shells[s].get_mean()) + " +- " + str(shells[s].get_stdev()));
      for (std::vector<size_t>::const_iterator v = shells[s].get_volumes().begin(); v != shells[s].get_volumes().end(); ++v)
        volumes.push_back (*v);
    }
    // Remove DW information from header if b=0 is the only 'shell' selected
    bzero = (shells.count() == 1 && shells[0].is_bzero());
  } else {
    const float bzero_threshold = File::Config::get_float ("BValueThreshold", 10.0);
    for (ssize_t row = 0; row != grad.rows(); ++row) {
      if ((bzero && (grad (row, 3) < bzero_threshold)) || (!bzero && (grad (row, 3) > bzero_threshold)))
        volumes.push_back (row);
    }
  }

  if (volumes.empty()) {
    auto type = (bzero) ? "b=0" : "dwi";
    throw Exception ("No " + str(type) + " volumes present");
  }

  std::sort (volumes.begin(), volumes.end());

  Header header (input_image);

  if (volumes.size() == 1)
    header.set_ndim (3);
  else
    header.size (3) = volumes.size();

  Eigen::MatrixXd new_grad (volumes.size(), grad.cols());
  for (size_t i = 0; i < volumes.size(); i++)
    new_grad.row (i) = grad.row (volumes[i]);
  header.set_DW_scheme (new_grad);

  auto output_image = Image<float>::create (argument[1], header);

  auto outer = Loop ("extracting volumes", input_image, 0, 3);

  if (output_image.ndim() == 4) {
    for (auto i = outer (output_image, input_image); i; ++i) {
      for (size_t i = 0; i < volumes.size(); i++) {
        input_image.index(3) = volumes[i];
        output_image.index(3) = i;
        output_image.value() = input_image.value();
      }
    }

  } else {
    const size_t volume = volumes[0];
    for (auto i = outer (output_image, input_image); i; ++i) {
      input_image.index(3) = volume;
      output_image.value() = input_image.value();
    }
  }
}