void Task::createPointcloudFromMLS(PCLPointCloudPtr pointcloud, envire::MultiLevelSurfaceGrid* mls_grid)
{
    pointcloud->clear();
    
    float vertical_distance = (mls_grid->getScaleX() + mls_grid->getScaleY()) * 0.5;
    if(vertical_distance <= 0.0)
        vertical_distance = 0.1;
    
    // create pointcloud from mls
    for(size_t x=0;x<mls_grid->getCellSizeX();x++)
    {
        for(size_t y=0;y<mls_grid->getCellSizeY();y++)
        {
            for( envire::MLSGrid::iterator cit = mls_grid->beginCell(x,y); cit != mls_grid->endCell(); cit++ )
            {
                envire::MLSGrid::SurfacePatch p( *cit );
                
                Eigen::Vector3d cellPosWorld = mls_grid->fromGrid(x, y, mls_grid->getEnvironment()->getRootNode());
                pcl::PointXYZ point;
                point.x = cellPosWorld.x();
                point.y = cellPosWorld.y();
                point.z = cellPosWorld.z();
                if(p.isHorizontal())
                {
                    point.z = cellPosWorld.z() + p.mean;
                    pointcloud->push_back(point);
                }
                else if(p.isVertical())
                {
                    float min_z = (float)p.getMinZ(0);
                    float max_z = (float)p.getMaxZ(0);
                    for(float z = min_z; z <= max_z; z += vertical_distance)
                    {
                        point.z = cellPosWorld.z() + z;
                        pointcloud->push_back(point);
                    }
                }
            }
        }
    }
}

//! Compute gravitational acceleration due to J2.
Eigen::Vector3d computeGravitationalAccelerationDueToJ2(
        const Eigen::Vector3d& positionOfBodySubjectToAcceleration,
        const double gravitationalParameterOfBodyExertingAcceleration,
        const double equatorialRadiusOfBodyExertingAcceleration,
        const double j2CoefficientOfGravityField,
        const Eigen::Vector3d& positionOfBodyExertingAcceleration )
{
    // Set constant values reused for optimal computation of acceleration components.
    const double distanceBetweenBodies = ( positionOfBodySubjectToAcceleration
                                           - positionOfBodyExertingAcceleration ).norm( );

    const double preMultiplier = -gravitationalParameterOfBodyExertingAcceleration
            / std::pow( distanceBetweenBodies, 4.0 ) * 1.5 * j2CoefficientOfGravityField
            * equatorialRadiusOfBodyExertingAcceleration
            * equatorialRadiusOfBodyExertingAcceleration;

    const double scaledZCoordinate = ( positionOfBodySubjectToAcceleration.z( )
                                       - positionOfBodyExertingAcceleration.z( ) )
            / distanceBetweenBodies;

    const double scaledZCoordinateSquared = scaledZCoordinate * scaledZCoordinate;

    const double factorForXAndYDirections = ( 1.0 - 5.0 * scaledZCoordinateSquared )
            / distanceBetweenBodies;

    // Compute components of acceleration due to J2-effect.
    Eigen::Vector3d gravitationalAccelerationDueToJ2 = Eigen::Vector3d::Constant( preMultiplier );

    gravitationalAccelerationDueToJ2( basic_astrodynamics::xCartesianPositionIndex )
            *= ( positionOfBodySubjectToAcceleration.x( )
                 - positionOfBodyExertingAcceleration.x( ) ) * factorForXAndYDirections;

    gravitationalAccelerationDueToJ2( basic_astrodynamics::yCartesianPositionIndex )
            *= ( positionOfBodySubjectToAcceleration.y( )
                 - positionOfBodyExertingAcceleration.y( ) ) * factorForXAndYDirections;

    gravitationalAccelerationDueToJ2( basic_astrodynamics::zCartesianPositionIndex )
            *= ( 3.0 - 5.0 * scaledZCoordinateSquared ) * scaledZCoordinate;

    return gravitationalAccelerationDueToJ2;
}

double
ClosingBoundary::getLineDistance (const Eigen::Vector3d &P0, const Eigen::Vector3d &u, const Eigen::Vector3d &Q0,
                                  const Eigen::Vector3d &v, Eigen::Vector3d &P, Eigen::Vector3d &Q)
{
  Eigen::Vector3d w0 = P0 - Q0;

  double a = u.dot (u);
  double b = u.dot (v);
  double c = v.dot (v);
  double d = u.dot (w0);
  double e = v.dot (w0);

  double s = (b * e - c * d) / (a * c - b * b);
  double t = (a * e - b * d) / (a * c - b * b);

  P = P0 + u * s;
  Q = Q0 + v * t;

  Eigen::Vector3d wc = P - Q;
  return wc.norm ();
}

//==============================================================================
Eigen::Isometry3d State::getCOMFrame() const
{
  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

  // Y-axis
  const Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY();

  // X-axis
  Eigen::Vector3d pelvisXAxis = mPelvis->getTransform().linear().col(0);
  const double mag = yAxis.dot(pelvisXAxis);
  pelvisXAxis -= mag * yAxis;
  const Eigen::Vector3d xAxis = pelvisXAxis.normalized();

  // Z-axis
  const Eigen::Vector3d zAxis = xAxis.cross(yAxis);

  T.translation() = getCOM();

  T.linear().col(0) = xAxis;
  T.linear().col(1) = yAxis;
  T.linear().col(2) = zAxis;

  return T;
}

	void vision_position_estimate(uint64_t usec,
			Eigen::Vector3d &position,
			Eigen::Vector3d &rpy)
	{
		mavlink::common::msg::VISION_POSITION_ESTIMATE vp{};

		vp.usec = usec;

		// [[[cog:
		// for f in "xyz":
		//     cog.outl("vp.%s = position.%s();" % (f, f))
		// for a, b in zip("xyz", ('roll', 'pitch', 'yaw')):
		//     cog.outl("vp.%s = rpy.%s();" % (b, a))
		// ]]]
		vp.x = position.x();
		vp.y = position.y();
		vp.z = position.z();
		vp.roll = rpy.x();
		vp.pitch = rpy.y();
		vp.yaw = rpy.z();
		// [[[end]]] (checksum: 2048daf411780847e77f08fe5a0b9dd3)

		UAS_FCU(m_uas)->send_message_ignore_drop(vp);
	}

  Line::Line(const Eigen::Vector3d& direction, const Eigen::Vector3d& origin)
    : direction_ (direction.normalized()), origin_(origin)
  {

  }

    visualization_msgs::Marker rviz_arrow(const Eigen::Vector3d & arrow, const Eigen::Vector3d & arrow_origin, int id, std::string name_space )
    {
        Eigen::Quaternion<double> rotation;
        if(arrow.norm()<0.0001)
        {
            rotation=Eigen::Quaternion<double>(1,0,0,0);
        }
        else
        {
            double rotation_angle=acos(arrow.normalized().dot(Eigen::Vector3d::UnitX()));
            Eigen::Vector3d rotation_axis=arrow.normalized().cross(Eigen::Vector3d::UnitX()).normalized();
            rotation=Eigen::AngleAxisd(-rotation_angle+PI,rotation_axis);
        }

        visualization_msgs::Marker marker;
        marker.header.frame_id = "/base_link";
        marker.header.stamp = ros::Time();
        marker.id = id;
        if(id==0)
        {
            marker.color.r = 0.0;
            marker.color.g = 0.0;
            marker.color.b = 1.0;
            marker.ns = name_space;
        }
        else
        {
            marker.color.r = 1.0;
            marker.color.g = 0.0;
            marker.color.b = 0.0;
            marker.ns = name_space;
        }
        marker.type = visualization_msgs::Marker::ARROW;
        marker.action = visualization_msgs::Marker::ADD;
        marker.pose.position.x = arrow_origin.x();
        marker.pose.position.y = arrow_origin.y();
        marker.pose.position.z = arrow_origin.z();
        marker.pose.orientation.x = rotation.x();
        marker.pose.orientation.y = rotation.y();
        marker.pose.orientation.z = rotation.z();
        marker.pose.orientation.w = rotation.w();
        //std::cout <<"position:" <<marker.pose.position << std::endl;
        //std::cout <<"orientation:" <<marker.pose.orientation << std::endl;
        //marker.pose.orientation.x = 0;
        //marker.pose.orientation.y = 0;
        //marker.pose.orientation.z = 0;
        //marker.pose.orientation.w = 1;
        if(arrow.norm()<0.0001)
        {
            marker.scale.x = 0.001;
            marker.scale.y = 0.001;
            marker.scale.z = 0.001;
        }
        else
        {
            marker.scale.x = arrow.norm();
            marker.scale.y = 0.1;
            marker.scale.z = 0.1;
        }
        marker.color.a = 1.0;


        return marker;
    }

void ArnoldAnimationPatch::getPositionFromAss(const set<Device*>& devices)
{
#ifdef USE_ARNOLD
  // by default in y-up coordinate systems, the light faces -z to start with.
  // The procedure here will be to grab the translation component of the arnold matrix
  // (which is actually just the position of the light in 3-d space) and set the
  // look at point to origin - (lookAtDir.normalized()) and the distance to 1.

  for (auto d : devices) {
    // bit of a hack, but don't touch quad light positions, they seem a bit odd
    if (d->getType() == "quad_light")
      continue;

    string nodeName = d->getMetadata("Arnold Node Name");
    AtNode* light = AiNodeLookUpByName(nodeName.c_str());
    
    if (light != nullptr) {
      // Get the matrix
      AtMatrix m;
      AiNodeGetMatrix(light, "matrix", m);

      // Get the light location
      Eigen::Vector3d origin(m[3][0], m[3][1], m[3][2]);

      // Get the rotation matrix
      Eigen::Matrix3d rot;
      rot(0, 0) = m[0][0];
      rot(0, 1) = m[0][1];
      rot(0, 2) = m[0][2];
      rot(1, 0) = m[1][0];
      rot(1, 1) = m[1][1];
      rot(1, 2) = m[1][2];
      rot(2, 0) = m[2][0];
      rot(2, 1) = m[2][1];
      rot(2, 2) = m[2][2];

      // rotate -z to get direction
      Eigen::Vector3d z(0, 0, -1);

      Eigen::Vector3d dir = z.transpose() * rot;
      Eigen::Vector3d look = origin + dir.normalized();

      // calculate spherical coords
      Eigen::Vector3d relPos = origin - look;
      double polar = acos(relPos(1) / relPos.norm()) * (180.0 / M_PI);
      double azimuth = atan2(relPos(2), relPos(0)) * (180 / M_PI);

      // Set params, and lock them to the values found in the file.
      // If the params don't exist, just create them
      if (!d->paramExists("distance"))
        d->setParam("distance", new LumiverseFloat());
      if (!d->paramExists("polar"))
        d->setParam("polar", new LumiverseOrientation());
      if (!d->paramExists("azimuth"))
        d->setParam("azimuth", new LumiverseOrientation());
      if (!d->paramExists("lookAtX"))
        d->setParam("lookAtX", new LumiverseFloat());
      if (!d->paramExists("lookAtY"))
        d->setParam("lookAtY", new LumiverseFloat());
      if (!d->paramExists("lookAtZ"))
        d->setParam("lookAtZ", new LumiverseFloat());

      d->getParam<LumiverseFloat>("distance")->setVals(1, 1, 1, 1);
      d->getParam<LumiverseOrientation>("polar")->setVals((float)polar, (float)polar, (float)polar, (float)polar);
      d->getParam<LumiverseOrientation>("azimuth")->setVals((float)azimuth, (float)azimuth, (float)azimuth, (float)azimuth);
      d->getParam<LumiverseFloat>("lookAtX")->setVals((float)look(0), (float)look(0), (float)look(0), (float)look(0));
      d->getParam<LumiverseFloat>("lookAtY")->setVals((float)look(1), (float)look(1), (float)look(1), (float)look(1));
      d->getParam<LumiverseFloat>("lookAtZ")->setVals((float)look(2), (float)look(2), (float)look(2), (float)look(2));
    }
  }
#endif
}

void Manipulator::computeJacobian(int32_t idx, Eigen::MatrixXd& J)
{
  J = Eigen::MatrixXd::Zero(6, dof_);

  if(idx < dof_) // Not required to consider end-effector
  {
    for(uint32_t i = 0; i <= idx; ++i)
    {
      if(link_[i]->ep) // joint_type is prismatic
      {
        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * Eigen::Vector3d::Zero();
      }
      else // joint_type is revolute
      {
        Eigen::Matrix4d Tib;
        math::calculateInverseTransformationMatrix(T_abs_[i], Tib);
        Eigen::Matrix4d Cin = Tib * C_abs_[idx];
        Eigen::Vector3d P = Cin.block(0, 3, 3, 1);

        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis().cross(P);
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
      }
    }
  }
  else // Required to consider the offset of end-effector
  {
    --idx;
    for(uint32_t i = 0; i <= idx; ++i)
    {
      if(link_[i]->ep) // joint_type is prismatic
      {
        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * Eigen::Vector3d::Zero();
      }
      else // joint_type is revolute
      {
        Eigen::Matrix4d Tib;
        math::calculateInverseTransformationMatrix(T_abs_[i], Tib);
        Eigen::Matrix4d Cin = Tib * C_abs_[idx];
        Eigen::Vector3d P = Cin.block(0, 3, 3, 1);

        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis().cross(P);
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
      }
    }

    Eigen::MatrixXd J_Pne = Eigen::MatrixXd::Identity(6, 6);
    Eigen::Vector3d Pne;
    if(C_abs_.size() - 1 - 1 >= 0.0)
    {
      Pne = T_abs_[T_abs_.size() - 1].block(0, 3, 3, 1) - C_abs_[C_abs_.size() - 1 - 1].block(0, 3, 3, 1);
    }
    else
    {
      std::stringstream msg;
      msg << "C_abs_.size() <= 1" << std::endl
          << "Manipulator doesn't have enough links." << std::endl;
      throw ahl_utils::Exception("Manipulator::computeJacobian", msg.str());
    }

    Eigen::Matrix3d Pne_cross;
    Pne_cross <<           0.0,  Pne.coeff(2), -Pne.coeff(1),
                 -Pne.coeff(2),           0.0,  Pne.coeff(0),
                  Pne.coeff(1), -Pne.coeff(0),           0.0;
    J_Pne.block(0, 3, 3, 3) = Pne_cross;
    J = J_Pne * J;
  }
}

float
pcl::visualization::viewScreenArea (
    const Eigen::Vector3d &eye, 
    const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, 
    const Eigen::Matrix4d &view_projection_matrix, int width, int height)
{
  Eigen::Vector4d bounding_box[8];
  bounding_box[0] = Eigen::Vector4d(min_bb.x (), min_bb.y (), min_bb.z (), 1.0);
  bounding_box[1] = Eigen::Vector4d(max_bb.x (), min_bb.y (), min_bb.z (), 1.0);
  bounding_box[2] = Eigen::Vector4d(max_bb.x (), max_bb.y (), min_bb.z (), 1.0);
  bounding_box[3] = Eigen::Vector4d(min_bb.x (), max_bb.y (), min_bb.z (), 1.0);
  bounding_box[4] = Eigen::Vector4d(min_bb.x (), min_bb.y (), max_bb.z (), 1.0);
  bounding_box[5] = Eigen::Vector4d(max_bb.x (), min_bb.y (), max_bb.z (), 1.0);
  bounding_box[6] = Eigen::Vector4d(max_bb.x (), max_bb.y (), max_bb.z (), 1.0);
  bounding_box[7] = Eigen::Vector4d(min_bb.x (), max_bb.y (), max_bb.z (), 1.0);

  // Compute 6-bit code to classify eye with respect to the 6 defining planes
  int pos = ((eye.x () < bounding_box[0].x ()) )  // 1 = left
      + ((eye.x () > bounding_box[6].x ()) << 1)  // 2 = right
      + ((eye.y () < bounding_box[0].y ()) << 2)  // 4 = bottom
      + ((eye.y () > bounding_box[6].y ()) << 3)  // 8 = top
      + ((eye.z () < bounding_box[0].z ()) << 4)  // 16 = front
      + ((eye.z () > bounding_box[6].z ()) << 5); // 32 = back

  // Look up number of vertices
  int num = hull_vertex_table[pos][6];
  if (num == 0)
  {
    return (float (width * height));
  }
    //return 0.0;


//  cout << "eye: " << eye.x() << " " << eye.y() << " " << eye.z() << endl;
//  cout << "min: " << bounding_box[0].x() << " " << bounding_box[0].y() << " " << bounding_box[0].z() << endl;
//
//  cout << "pos: " << pos << " ";
//  switch(pos){
//    case 0:  cout << "inside" << endl; break;
//    case 1:  cout << "left" << endl; break;
//    case 2:  cout << "right" << endl; break;
//    case 3:
//    case 4:  cout << "bottom" << endl; break;
//    case 5:  cout << "bottom, left" << endl; break;
//    case 6:  cout << "bottom, right" << endl; break;
//    case 7:
//    case 8:  cout << "top" << endl; break;
//    case 9:  cout << "top, left" << endl; break;
//    case 10:  cout << "top, right" << endl; break;
//    case 11:
//    case 12:
//    case 13:
//    case 14:
//    case 15:
//    case 16:  cout << "front" << endl; break;
//    case 17:  cout << "front, left" << endl; break;
//    case 18:  cout << "front, right" << endl; break;
//    case 19:
//    case 20:  cout << "front, bottom" << endl; break;
//    case 21:  cout << "front, bottom, left" << endl; break;
//    case 22:
//    case 23:
//    case 24:  cout << "front, top" << endl; break;
//    case 25:  cout << "front, top, left" << endl; break;
//    case 26:  cout << "front, top, right" << endl; break;
//    case 27:
//    case 28:
//    case 29:
//    case 30:
//    case 31:
//    case 32:  cout << "back" << endl; break;
//    case 33:  cout << "back, left" << endl; break;
//    case 34:  cout << "back, right" << endl; break;
//    case 35:
//    case 36:  cout << "back, bottom" << endl; break;
//    case 37:  cout << "back, bottom, left" << endl; break;
//    case 38:  cout << "back, bottom, right" << endl; break;
//    case 39:
//    case 40:  cout << "back, top" << endl; break;
//    case 41:  cout << "back, top, left" << endl; break;
//    case 42:  cout << "back, top, right" << endl; break;
//  }

  //return -1 if inside
  Eigen::Vector2d dst[8];
  for (int i = 0; i < num; i++)
  {
    Eigen::Vector4d world_pt = bounding_box[hull_vertex_table[pos][i]];
    Eigen::Vector2i screen_pt = pcl::visualization::worldToView(world_pt, view_projection_matrix, width, height);
//    cout << "point[" << i << "]: " << screen_pt.x() << " " << screen_pt.y() << endl;
    dst[i] = Eigen::Vector2d(screen_pt.x (), screen_pt.y ());
  }

  double sum = 0.0;
  for (int i = 0; i < num; ++i)
  {
    sum += (dst[i].x () - dst[(i+1) % num].x ()) * (dst[i].y () + dst[(i+1) % num].y ());
  }

  return (fabsf (float (sum * 0.5f)));
//.........这里部分代码省略.........

int
pcl::visualization::cullFrustum (double frustum[24], const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb)
{
  int result = PCL_INSIDE_FRUSTUM;

  for(int i =0; i < 6; i++){
    double a = frustum[(i*4)];
    double b = frustum[(i*4)+1];
    double c = frustum[(i*4)+2];
    double d = frustum[(i*4)+3];

    //cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << endl;

    //  Basic VFC algorithm
    Eigen::Vector3d center ((max_bb.x () - min_bb.x ()) / 2 + min_bb.x (),
                            (max_bb.y () - min_bb.y ()) / 2 + min_bb.y (),
                            (max_bb.z () - min_bb.z ()) / 2 + min_bb.z ());

    Eigen::Vector3d radius (fabs (static_cast<double> (max_bb.x () - center.x ())),
                            fabs (static_cast<double> (max_bb.y () - center.y ())),
                            fabs (static_cast<double> (max_bb.z () - center.z ())));

    double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
    double n = (radius.x () * fabs(a)) + (radius.y () * fabs(b)) + (radius.z () * fabs(c));

    if (m + n < 0){
      result = PCL_OUTSIDE_FRUSTUM;
      break;
    }

    if (m - n < 0)
    {
      result = PCL_INTERSECT_FRUSTUM;
    }
  }

  return result;
}

//==============================================================================
std::string toString(const Eigen::Vector3d& _v)
{
  return boost::lexical_cast<std::string>(_v.transpose());
}

void LinearSystem::_BuildSystem(
        LinearSystem*       pLS,
        const unsigned int& StartU,
        const unsigned int& EndU,
        const unsigned int& StartV,
        const unsigned int& EndV
        )
{
    // Jacobian
    Eigen::Matrix<double, 1, 6> BigJ;
    Eigen::Matrix<double, 1, 6> J;

    BigJ.setZero();
    J.setZero();

	// Errors
	double		 e;
	double		 SSD = 0;
    double       Error    = 0;
    unsigned int ErrorPts = 0;

    for( int ii = StartV; ii < EndV; ii++ ) {
        for( int jj = StartU; jj < EndU; jj++ ) {
            // variables
            Eigen::Vector2d Ur;        // pixel position
            Eigen::Vector3d Pr, Pv;    // 3d point
            Eigen::Vector4d Ph;        // homogenized point

            // check if pixel is contained in our model (i.e. has depth)
            if( pLS->m_vVirtDepth[ii * pLS->m_nImgWidth + jj] == 0 ) {
                continue;
            }

            // --------------------- first term 1x2
            // evaluate 'a' = L[ Trv * Linv( Uv ) ]
            // back project to virtual camera's reference frame
            // this already brings points to robotics reference frame
            Pv = pLS->_BackProject( jj, ii, pLS->m_vVirtDepth[ii * pLS->m_nImgWidth + jj] );

            // convert to homogeneous coordinate
            Ph << Pv, 1;

            // transform point to reference camera's frame
            // Pr = Trv * Pv
            Ph = pLS->m_dTrv * Ph;
            Pr = Ph.head( 3 );

            // project onto reference camera
            Eigen::Vector3d Lr;

            Lr = pLS->_Project( Pr );
            Ur = Lr.head( 2 );
            Ur = Ur / Lr( 2 );

            // check if point falls in camera's field of view
            if( (Ur( 0 ) <= 1) || (Ur( 0 ) >= pLS->m_nImgWidth - 2) || (Ur( 1 ) <= 1)
                    || (Ur( 1 ) >= pLS->m_nImgHeight - 2) ) {
                continue;
            }

            // finite differences
            float                       TopPix   = pLS->_Interpolate( Ur( 0 ), Ur( 1 ) - 1, pLS->m_vRefImg );
            float                       BotPix   = pLS->_Interpolate( Ur( 0 ), Ur( 1 ) + 1, pLS->m_vRefImg );
            float                       LeftPix  = pLS->_Interpolate( Ur( 0 ) - 1, Ur( 1 ), pLS->m_vRefImg );
            float                       RightPix = pLS->_Interpolate( Ur( 0 ) + 1, Ur( 1 ), pLS->m_vRefImg );
            Eigen::Matrix<double, 1, 2> Term1;

            Term1( 0 ) = (RightPix - LeftPix) / 2.0;
            Term1( 1 ) = (BotPix - TopPix) / 2.0;

            // --------------------- second term 2x3
            // evaluate 'b' = Trv * Linv( Uv )
            // this was already calculated for Term1
            // fill matrix
            // 1/c      0       -a/c^2
            // 0       1/c     -b/c^2
            Eigen::Matrix<double, 2, 3> Term2;
            double                      PowC = Lr( 2 ) * Lr( 2 );

            Term2( 0, 0 ) = 1.0 / Lr( 2 );
            Term2( 0, 1 ) = 0;
            Term2( 0, 2 ) = -(Lr( 0 )) / PowC;
            Term2( 1, 0 ) = 0;
            Term2( 1, 1 ) = 1.0 / Lr( 2 );
            Term2( 1, 2 ) = -(Lr( 1 )) / PowC;
            Term2         = Term2 * pLS->m_Kr;

            // --------------------- third term 3x1
            // we need Pv in homogenous coordinates
            Ph << Pv, 1;

            Eigen::Vector4d Term3i;

            // last row of Term3 is truncated since it is always 0
            Eigen::Vector3d Term3;

            // fill Jacobian with T generators
            Term3i    = pLS->m_dTrv * pLS->m_Gen[0] * Ph;
            Term3     = Term3i.head( 3 );
            J( 0, 0 ) = Term1 * Term2 * Term3;
//.........这里部分代码省略.........

 inline double SlaterSet::pointSlater(SlaterSet *set, const Eigen::Vector3d &delta,
                     const double &dr, unsigned int slater, unsigned int indexMO,
                     double expZeta)
 {
   if (isSmall(set->m_normalized.coeffRef(slater, indexMO))) return 0.0;
   double tmp = set->m_normalized.coeffRef(slater, indexMO) * expZeta;
   // Radial part with effective PQNs
   for (int i = 0; i < set->m_PQNs[slater]; ++i)
     tmp *= dr;
   switch (set->m_slaterTypes[slater]) {
     case S:
       break;
     case PX:
       tmp *= delta.x();
       break;
     case PY:
       tmp *= delta.y();
       break;
     case PZ:
       tmp *= delta.z();
       break;
     case X2: // (x^2 - y^2)r^n
       tmp *= delta.x() * delta.x() - delta.y() * delta.y();
       break;
     case XZ: // xzr^n
       tmp *= delta.x() * delta.z();
       break;
     case Z2: // (2z^2 - x^2 - y^2)r^n
       tmp *= 2.0 * delta.z() * delta.z() - delta.x() * delta.x()
            - delta.y() * delta.y();
       break;
     case YZ: // yzr^n
       tmp *= delta.y() * delta.z();
       break;
     case XY: // xyr^n
       tmp *= delta.x() * delta.y();
       break;
     default:
       return 0.0;
   }
   return tmp;
 }

float PlaneFit::Fit()
{
    _bIsFitted = true;
    if (CountPoints() < 3)
        return FLOAT_MAX;

    double sxx,sxy,sxz,syy,syz,szz,mx,my,mz;
    sxx=sxy=sxz=syy=syz=szz=mx=my=mz=0.0f;

    for (std::list<Base::Vector3f>::iterator it = _vPoints.begin(); it!=_vPoints.end(); ++it) {
        sxx += it->x * it->x; sxy += it->x * it->y;
        sxz += it->x * it->z; syy += it->y * it->y;
        syz += it->y * it->z; szz += it->z * it->z;
        mx  += it->x;   my += it->y;   mz += it->z;
    }

    unsigned int nSize = _vPoints.size();
    sxx = sxx - mx*mx/((double)nSize);
    sxy = sxy - mx*my/((double)nSize);
    sxz = sxz - mx*mz/((double)nSize);
    syy = syy - my*my/((double)nSize);
    syz = syz - my*mz/((double)nSize);
    szz = szz - mz*mz/((double)nSize);

#if defined(FC_USE_BOOST)
    ublas::matrix<double> A(3,3);
    A(0,0) = sxx;
    A(1,1) = syy;
    A(2,2) = szz;
    A(0,1) = sxy; A(1,0) = sxy;
    A(0,2) = sxz; A(2,0) = sxz;
    A(1,2) = syz; A(2,1) = syz;
    namespace lapack= boost::numeric::bindings::lapack;
    ublas::vector<double> eigenval(3);
    int r = lapack::syev('V','U',A,eigenval,lapack::optimal_workspace());
    if (r) {
    }
    float sigma = 0;
#elif defined(FC_USE_EIGEN)
    Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
    covMat(0,0) = sxx;
    covMat(1,1) = syy;
    covMat(2,2) = szz;
    covMat(0,1) = sxy; covMat(1,0) = sxy;
    covMat(0,2) = sxz; covMat(2,0) = sxz;
    covMat(1,2) = syz; covMat(2,1) = syz;
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(covMat);

    Eigen::Vector3d u = eig.eigenvectors().col(1);
    Eigen::Vector3d v = eig.eigenvectors().col(2);
    Eigen::Vector3d w = eig.eigenvectors().col(0);

    _vDirU.Set(u.x(), u.y(), u.z());
    _vDirV.Set(v.x(), v.y(), v.z());
    _vDirW.Set(w.x(), w.y(), w.z());
    _vBase.Set(mx/(float)nSize, my/(float)nSize, mz/(float)nSize);

    float sigma = w.dot(covMat * w);
#else
    // Covariance matrix
    Wm4::Matrix3<double> akMat(sxx,sxy,sxz,sxy,syy,syz,sxz,syz,szz);
    Wm4::Matrix3<double> rkRot, rkDiag;
    try {
        akMat.EigenDecomposition(rkRot, rkDiag);
    }
    catch (const std::exception&) {
        return FLOAT_MAX;
    }

    // We know the Eigenvalues are ordered
    // rkDiag(0,0) <= rkDiag(1,1) <= rkDiag(2,2)
    //
    // points describe a line or even are identical
    if (rkDiag(1,1) <= 0)
        return FLOAT_MAX;

    Wm4::Vector3<double> U = rkRot.GetColumn(1);
    Wm4::Vector3<double> V = rkRot.GetColumn(2);
    Wm4::Vector3<double> W = rkRot.GetColumn(0);

    // It may happen that the result have nan values
    for (int i=0; i<3; i++) {
        if (boost::math::isnan(U[i]) || 
            boost::math::isnan(V[i]) ||
            boost::math::isnan(W[i]))
            return FLOAT_MAX;
    }

    _vDirU.Set((float)U.X(), (float)U.Y(), (float)U.Z());
    _vDirV.Set((float)V.X(), (float)V.Y(), (float)V.Z());
    _vDirW.Set((float)W.X(), (float)W.Y(), (float)W.Z());
    _vBase.Set((float)(mx/nSize), (float)(my/nSize), (float)(mz/nSize));
    float sigma = (float)W.Dot(akMat * W);
#endif

    // In case sigma is nan
    if (boost::math::isnan(sigma))
        return FLOAT_MAX;

    // This must be caused by some round-off errors. Theoretically it's impossible
//.........这里部分代码省略.........

 inline double SlaterSet::calcSlater(SlaterSet *set, const Eigen::Vector3d &delta,
                     const double &dr, unsigned int slater)
 {
   double tmp = set->m_factors[slater] * exp(- set->m_zetas[slater] * dr);
   // Radial part with effective PQNs
   for (int i = 0; i < set->m_PQNs[slater]; ++i)
     tmp *= dr;
   switch (set->m_slaterTypes[slater]) {
     case S:
       break;
     case PX:
       tmp *= delta.x();
       break;
     case PY:
       tmp *= delta.y();
       break;
     case PZ:
       tmp *= delta.z();
       break;
     case X2: // (x^2 - y^2)r^n
       tmp *= delta.x() * delta.x() - delta.y() * delta.y();
       break;
     case XZ: // xzr^n
       tmp *= delta.x() * delta.z();
       break;
     case Z2: // (2z^2 - x^2 - y^2)r^n
       tmp *= 2.0 * delta.z() * delta.z() - delta.x() * delta.x()
            - delta.y() * delta.y();
       break;
     case YZ: // yzr^n
       tmp *= delta.y() * delta.z();
       break;
     case XY: // xyr^n
       tmp *= delta.x() * delta.y();
       break;
     default:
       return 0.0;
   }
   return tmp;
 }

void
FittingCurve2dTDM::assembleInterior (double wInt, double sigma2, unsigned &row)
{
  int nInt = m_data->interior.size ();
  bool wFunction (true);
  double ds = 1.0 / (2.0 * sigma2);
  m_data->interior_line_start.clear ();
  m_data->interior_line_end.clear ();
  m_data->interior_error.clear ();
  m_data->interior_normals.clear ();
  for (int p = 0; p < nInt; p++)
  {
    Eigen::Vector2d &pcp = m_data->interior[p];

    // inverse mapping
    double param;
    Eigen::Vector2d pt, t, n;
    double error;
    if (p < (int)m_data->interior_param.size ())
    {
      param = inverseMapping (m_nurbs, pcp, m_data->interior_param[p], error, pt, t, in_max_steps, in_accuracy);
      m_data->interior_param[p] = param;
    }
    else
    {
      param = findClosestElementMidPoint(m_nurbs, pcp);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, in_max_steps, in_accuracy);
      m_data->interior_param.push_back (param);
    }

    m_data->interior_error.push_back (error);

    double pointAndTangents[6];
    m_nurbs.Evaluate (param, 2, 2, pointAndTangents);
    pt (0) = pointAndTangents[0];
    pt (1) = pointAndTangents[1];
    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];
    n (0) = pointAndTangents[4];
    n (1) = pointAndTangents[5];

    // evaluate if point lies inside or outside the closed curve
    Eigen::Vector3d a (pcp (0) - pt (0), pcp (1) - pt (1), 0.0);
    Eigen::Vector3d b (t (0), t (1), 0.0);
    Eigen::Vector3d z = a.cross (b);

    if (p < (int)m_data->interior_weight.size ())
      wInt = m_data->interior_weight[p];

    if (p < (int)m_data->interior_weight_function.size ())
      wFunction = m_data->interior_weight_function[p];

    double w (wInt);
    if (z (2) > 0.0 && wFunction)
      w = wInt * exp (-(error * error) * ds);

    n.normalize ();
    //    m_data->interior_line_start.push_back(pt);
    //    m_data->interior_line_end.push_back(pt + n * 0.01);

    //      w = 0.5 * wInt * exp(-(error * error) * ds);

    // evaluate if this point is the closest point
    //    int idx = NurbsTools::getClosestPoint(pt, m_data->interior);
    //    if(idx == p)
    //      w = 2.0 * wInt;

    if (w > 1e-6) // avoids ill-conditioned matrix
      addPointConstraint (m_data->interior_param[p], m_data->interior[p], n, w, row);
    else
    {
      //      m_solver.K(row, 0, 0.0);
      //      row++;
    }
  }
}

int main(int argc, char** argv){

  po::options_description desc("./cvt2pcd [options] input_cloud output_pcd");

  desc.add_options()
          ("help", "produce help message")
          ("input,i",po::value<std::string>()->required(), "input point cloud ")
          ("output,o",po::value<std::string>(), "output pcd ")
          ("view_offset,v", po::value< std::string>(), "Offset view offset to translate and convert a float64 to float32 XYZ")
          ("ascii,a", "PCD should be in asci format.  (Default binary compressed)")
          ;

  if (argc <3 ){
           std::cout << "Incorrect number of arguments\n";
           desc.print(std::cout);
           return -1;
   }
  po::positional_options_description p;
  p.add("input",1);
  p.add("output",1);

  po::variables_map vm;
 try{
  po::store(po::command_line_parser(argc, argv).
  options(desc).positional(p).run(), vm);
  po::notify(vm);
 }
 catch( const std::exception& e)
 {
     std::cerr << e.what() << std::endl;
     std::cout << desc << std::endl;
     return 1;
 }

  pcl::CloudReader reader;

  std::string ifile, ofile;
  ifile = vm["input"].as<std::string>();
  if(vm.count("output")) ofile = vm["output"].as<std::string>();
  else {
    ofile = ifile;
    boost::filesystem::path opath = ofile;
    opath.replace_extension(".pcd");
    ofile = opath.string();
  }

#ifdef E57
  std::cout << "Adding E57 Support : \n";
  reader.registerExtension("e57", new pcl::E57Reader( ));
#endif
#ifdef LAS
  reader.registerExtension("las", new pcl::LASReader );
#endif
  pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);

  Eigen::Vector4f origin;
  Eigen::Quaternionf rot;

  int fv;
  std::clog << "Loading " <<  ifile << " \n";
  int c = reader.read(ifile,*cloud, origin, rot, fv);
  std::clog << "There were " << c << " points \n";

  if (pcl::hasDoublePointXYZ(*cloud) ){
    if (vm.count("view_offset")){
      Eigen::Vector3d voff;

      if ( 3 == sscanf(vm["view_offset"].as<std::string>().c_str(), "%lf,%lf,%lf", &voff[0], &voff[1], &voff[2]) ){
        std::cout << "Translating by " << voff.transpose() << " \n";
        pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
        pcl::cvtToDoubleAndOffset(*cloud,*cloud2,voff);
        cloud = cloud2;
      }
      else{
        std::cout << "Invalid view offset of : " << vm["view_offset"].as<std::string>() << " \n";
      }
    }
  }


  pcl::PCDWriter writer;
  writer.write(ofile, cloud,origin, rot, !vm.count("ascii"));
  std::cout << ofile << "\n";
}

template<typename PointT> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud, 
                                                                                    const typename pcl::search::KdTree<PointT>::Ptr kdtree,
                                                                                    std::vector<Eigen::Matrix3d>& cloud_covariances)
{
  if (k_correspondences_ > int (cloud->size ()))
  {
    PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_);
    return;
  }

  Eigen::Vector3d mean;
  std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
  std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);

  // We should never get there but who knows
  if(cloud_covariances.size () < cloud->size ())
    cloud_covariances.resize (cloud->size ());

  typename pcl::PointCloud<PointT>::const_iterator points_iterator = cloud->begin ();
  std::vector<Eigen::Matrix3d>::iterator matrices_iterator = cloud_covariances.begin ();
  for(;
      points_iterator != cloud->end ();
      ++points_iterator, ++matrices_iterator)
  {
    const PointT &query_point = *points_iterator;
    Eigen::Matrix3d &cov = *matrices_iterator;
    // Zero out the cov and mean
    cov.setZero ();
    mean.setZero ();

    // Search for the K nearest neighbours
    kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
    
    // Find the covariance matrix
    for(int j = 0; j < k_correspondences_; j++) {
      const PointT &pt = (*cloud)[nn_indecies[j]];
      
      mean[0] += pt.x;
      mean[1] += pt.y;
      mean[2] += pt.z;
      
      cov(0,0) += pt.x*pt.x;
      
      cov(1,0) += pt.y*pt.x;
      cov(1,1) += pt.y*pt.y;
      
      cov(2,0) += pt.z*pt.x;
      cov(2,1) += pt.z*pt.y;
      cov(2,2) += pt.z*pt.z;    
    }
  
    mean /= static_cast<double> (k_correspondences_);
    // Get the actual covariance
    for (int k = 0; k < 3; k++)
      for (int l = 0; l <= k; l++) 
      {
        cov(k,l) /= static_cast<double> (k_correspondences_);
        cov(k,l) -= mean[k]*mean[l];
        cov(l,k) = cov(k,l);
      }
    
    // Compute