inline void resize(Eigen::MatrixXd & m,
                    vcl_size_t new_rows,
                    vcl_size_t new_cols)
 {
   m.resize(new_rows, new_cols);
 }

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

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

/** \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]++;
      }
    }
  }
  
}

bool GaussianSet::setDensityMatrix(const Eigen::MatrixXd &m)
{
  m_density.resize(m.rows(), m.cols());
  m_density = m;
  return true;
}

/*! \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;
            }
        }
    }
}

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

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

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

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();

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

 inline vcl_size_t size2(Eigen::MatrixXd const & m) { return m.cols(); }

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

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

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

/**
 * 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();
}

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

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