void Element::CalculateStiffnessMatrix(const Eigen::Matrix3f& D, std::vector<Eigen::Triplet<float> >& triplets)
{
	Eigen::Vector3f x, y;
	x << nodesX[nodesIds[0]], nodesX[nodesIds[1]], nodesX[nodesIds[2]];
	y << nodesY[nodesIds[0]], nodesY[nodesIds[1]], nodesY[nodesIds[2]];
	
	Eigen::Matrix3f C;
	C << Eigen::Vector3f(1.0f, 1.0f, 1.0f), x, y;
	
	Eigen::Matrix3f IC = C.inverse();

	for (int i = 0; i < 3; i++)
	{
		B(0, 2 * i + 0) = IC(1, i);
		B(0, 2 * i + 1) = 0.0f;
		B(1, 2 * i + 0) = 0.0f;
		B(1, 2 * i + 1) = IC(2, i);
		B(2, 2 * i + 0) = IC(2, i);
		B(2, 2 * i + 1) = IC(1, i);
	}
	Eigen::Matrix<float, 6, 6> K = B.transpose() * D * B * C.determinant() / 2.0f;

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			Eigen::Triplet<float> trplt11(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 0, K(2 * i + 0, 2 * j + 0));
			Eigen::Triplet<float> trplt12(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 1, K(2 * i + 0, 2 * j + 1));
			Eigen::Triplet<float> trplt21(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 0, K(2 * i + 1, 2 * j + 0));
			Eigen::Triplet<float> trplt22(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 1, K(2 * i + 1, 2 * j + 1));

			triplets.push_back(trplt11);
			triplets.push_back(trplt12);
			triplets.push_back(trplt21);
			triplets.push_back(trplt22);
		}
	}
}

bool PositionBasedElasticRod::ComputeDarbouxVector(const Eigen::Matrix3f& dA, const Eigen::Matrix3f& dB, const float mid_edge_length, Eigen::Vector3f& darboux_vector)
{

	float factor = 1.0f + dA.col(0).dot(dB.col(0)) + dA.col(1).dot(dB.col(1)) + dA.col(2).dot(dB.col(2));

	factor = 2.0f / (mid_edge_length * factor);

	for (int c = 0; c < 3; ++c)
	{
		const int i = permutation[c][0];
		const int j = permutation[c][1];
		const int k = permutation[c][2];

		darboux_vector[i] = dA.col(j).dot(dB.col(k)) - dA.col(k).dot(dB.col(j));
	}

	darboux_vector *= factor;
	return true;
}

void transformOrientationToGroundFrame(const VirtualRobot::RobotPtr& robot,
                                       const Eigen::Matrix6Xf& leftFootTrajectory,
                                       const VirtualRobot::RobotNodePtr& leftFoot,
                                       const VirtualRobot::RobotNodePtr& rightFoot,
                                       const VirtualRobot::RobotNodeSetPtr& bodyJoints,
                                       const Eigen::MatrixXf& bodyTrajectory,
                                       const Eigen::Matrix3Xf& trajectory,
                                       const std::vector<SupportInterval>& intervals,
                                       Eigen::Matrix3Xf& relativeTrajectory)
{
    Eigen::Matrix4f leftInitialPose = bodyJoints->getKinematicRoot()->getGlobalPose();
    int N = trajectory.cols();
    int M = trajectory.rows();
    relativeTrajectory.resize(M, N);

    BOOST_ASSERT(M > 0 && M <= 3);

    auto intervalIter = intervals.begin();
    for (int i = 0; i < N; i++)
    {
        while (i >= intervalIter->endIdx)
        {
            intervalIter = std::next(intervalIter);
        }
        // Move basis along with the left foot
        Eigen::Matrix4f leftFootPose = Eigen::Matrix4f::Identity();
        VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1),
                                                leftFootTrajectory.block(3, i, 3, 1),  leftFootPose);
        robot->setGlobalPose(leftFootPose);
        bodyJoints->setJointValues(bodyTrajectory.col(i));
        Eigen::Matrix3f worldToRef = computeGroundFrame(leftFoot->getGlobalPose(),
                                                        rightFoot->getGlobalPose(),
                                                        intervalIter->phase).block(0, 0, 3, 3);
        relativeTrajectory.block(0, i, M, 1) = worldToRef.colPivHouseholderQr().solve(trajectory.col(i)).block(0, 0, M, 1);
    }
}

template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD (
    const pcl::PointCloud<PointT> &cloud_src, 
    const std::vector<int> &indices_src, 
    const pcl::PointCloud<PointT> &cloud_tgt,
    const std::vector<int> &indices_tgt, 
    Eigen::VectorXf &transform)
{
  transform.resize (16);
  Eigen::Vector4f centroid_src, centroid_tgt;
  // Estimate the centroids of source, target
  compute3DCentroid (cloud_src, indices_src, centroid_src);
  compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);

  // Subtract the centroids from source, target
  Eigen::MatrixXf cloud_src_demean;
  demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);

  Eigen::MatrixXf cloud_tgt_demean;
  demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);

  // Assemble the correlation matrix H = source * target'
  Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();

  // Compute the Singular Value Decomposition
  Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3f u = svd.matrixU ();
  Eigen::Matrix3f v = svd.matrixV ();

  // Compute R = V * U'
  if (u.determinant () * v.determinant () < 0)
  {
    for (int x = 0; x < 3; ++x)
      v (x, 2) *= -1;
  }

  Eigen::Matrix3f R = v * u.transpose ();

  // Return the correct transformation
  transform.segment<3> (0) = R.row (0); transform[12]  = 0;
  transform.segment<3> (4) = R.row (1); transform[13]  = 0;
  transform.segment<3> (8) = R.row (2); transform[14] = 0;

  Eigen::Vector3f t = centroid_tgt.head<3> () - R * centroid_src.head<3> ();
  transform[3] = t[0]; transform[7] = t[1]; transform[11] = t[2]; transform[15] = 1.0;
}

// ----------------------------------------------------------------------------------------------
bool PositionBasedElasticRod::ComputeMaterialFrame(
	const Eigen::Vector3f& pA, 
	const Eigen::Vector3f& pB, 
	const Eigen::Vector3f& pG, 
	Eigen::Matrix3f& frame)
{

	frame.col(2) = (pB - pA);
	frame.col(2).normalize();

	frame.col(1) = (frame.col(2).cross(pG - pA));
	frame.col(1).normalize();

	frame.col(0) = frame.col(1).cross(frame.col(2));
//	frame.col(0).normalize();
	return true;
}

bool FruitDetector::enhanceAlignment(
  const Eigen::Matrix3f& new_rotation, const boost::shared_ptr<Fruit>& f) {
  // get euler angles. (Z-Y-X) convention --> (Yaw, Pitch, Roll)
  const Eigen::Vector3f euler_angles = new_rotation.eulerAngles(2, 1, 0);
  const float new_yaw = euler_angles[0];
  const float new_pitch = euler_angles[1];
  const float new_roll = euler_angles[2];

  const float old_roll = f->get_RPY().roll;
  const float old_pitch = f->get_RPY().pitch;
  // check if the new aligned model is less tilted
  if (new_roll < old_roll && new_pitch < old_pitch)
    return true;
  return false;
}

bool ObjectFinder::find_toy_block(float surface_height, geometry_msgs::PoseStamped &object_pose) {
    Eigen::Vector3f plane_normal;
    double plane_dist;
    //bool valid_plane;
    Eigen::Vector3f major_axis;
    Eigen::Vector3f centroid;
    bool found_object = true; //should verify this
    double block_height = 0.035; //this height is specific to the TOY_BLOCK model
    //if insufficient points in plane, find_plane_fit returns "false"
    //should do more sanity testing on found_object status
    //hard-coded search bounds based on a block of width 0.035
    found_object = pclUtils_.find_plane_fit(0.4, 1, -0.5, 0.5, surface_height + 0.025, surface_height + 0.045, 0.001,
            plane_normal, plane_dist, major_axis, centroid);
    //should have put a return value on find_plane_fit;
    //
    if (plane_normal(2) < 0) plane_normal(2) *= -1.0; //in world frame, normal must point UP
    Eigen::Matrix3f R;
    Eigen::Vector3f y_vec;
    R.col(0) = major_axis;
    R.col(2) = plane_normal;
    R.col(1) = plane_normal.cross(major_axis);
    Eigen::Quaternionf quat(R);
    object_pose.header.frame_id = "base_link";
    object_pose.pose.position.x = centroid(0);
    object_pose.pose.position.y = centroid(1);
    //the TOY_BLOCK model has its origin in the middle of the block, not the top surface
    //so lower the block model origin by half the block height from upper surface
    object_pose.pose.position.z = centroid(2)-0.5*block_height;
    //create R from normal and major axis, then convert R to quaternion

    object_pose.pose.orientation.x = quat.x();
    object_pose.pose.orientation.y = quat.y();
    object_pose.pose.orientation.z = quat.z();
    object_pose.pose.orientation.w = quat.w();
    return found_object;
}

std::vector<Point3f> PnPUtil::BackprojectPts(const std::vector<Point2f>& pts, const Eigen::Matrix4f& camTf, const Eigen::Matrix3f& K, const Mat& depth)
{
  std::vector<Point3f> pts3d;
  for(std::size_t i = 0; i < pts.size(); i++)
  {
    Point2f pt = pts[i];
    Eigen::Vector3f hkp(pt.x, pt.y, 1);
    Eigen::Vector3f backproj = K.inverse()*hkp;
    backproj /= backproj(2);    
    backproj *= depth.at<float>(pt.y, pt.x);
    if(depth.at<float>(pt.y, pt.x) == 0)
      std::cout << "Bad depth (0)" << std::endl;
    Eigen::Vector4f backproj_h(backproj(0), backproj(1), backproj(2), 1);
    backproj_h = camTf*backproj_h;
    pts3d.push_back(Point3f(backproj_h(0), backproj_h(1), backproj_h(2)));
  }
  return pts3d;
}

void SQ_fitter<PointT>::getBoundingBox(const PointCloudPtr &_cloud,
				       double _dim[3],
				       double _trans[3],
				       double _rot[3] ) {

  // 1. Compute the bounding box center
  Eigen::Vector4d centroid;
  pcl::compute3DCentroid( *_cloud, centroid );
  _trans[0] = centroid(0);
  _trans[1] = centroid(1); 
  _trans[2] = centroid(2);

  // 2. Compute main axis orientations
  pcl::PCA<PointT> pca;
  pca.setInputCloud( _cloud );
  Eigen::Vector3f eigVal = pca.getEigenValues();
  Eigen::Matrix3f eigVec = pca.getEigenVectors();
  // Make sure 3 vectors are normal w.r.t. each other
  Eigen::Vector3f temp;
  eigVec.col(2) = eigVec.col(0); // Z
  Eigen::Vector3f v3 = (eigVec.col(1)).cross( eigVec.col(2) );
  eigVec.col(0) = v3;
  Eigen::Vector3f rpy = eigVec.eulerAngles(2,1,0);
 
  _rot[0] = (double)rpy(2);
  _rot[1] = (double)rpy(1);
  _rot[2] = (double)rpy(0);

  // Transform _cloud
  Eigen::Matrix4f transf = Eigen::Matrix4f::Identity();
  transf.block(0,3,3,1) << (float)centroid(0), (float)centroid(1), (float)centroid(2);
  transf.block(0,0,3,3) = eigVec;

  Eigen::Matrix4f tinv; tinv = transf.inverse();
  PointCloudPtr cloud_temp( new pcl::PointCloud<PointT>() );
  pcl::transformPointCloud( *_cloud, *cloud_temp, tinv );

  // Get maximum and minimum
  PointT minPt; PointT maxPt;
  pcl::getMinMax3D( *cloud_temp, minPt, maxPt );
  
  _dim[0] = ( maxPt.x - minPt.x ) / 2.0;
  _dim[1] = ( maxPt.y - minPt.y ) / 2.0;
  _dim[2] = ( maxPt.z - minPt.z ) / 2.0;

}

pcl::PointCloud<pcl::PointXYZRGB> createPointCloud(
  const Eigen::Matrix3f K, const cv::Mat &depth, const cv::Mat &color
) {
  pcl::PointCloud<pcl::PointXYZRGB> pointCloud;

  int w = depth.cols;
  int h = depth.rows;

  float *pDepth = (float*)depth.data;
  float *pColor = (float*)color.data;

  for (int y = 0; y < h; ++y) {
    for (int x = 0; x < w; ++x) {
      size_t off = (y*w + x);
      size_t off3 = off * 3;

      // Check depth value validity
      float d = pDepth[x + y*w];
      if (d == 0.0f || std::isnan(d)) { continue; }

      // RGBXYZ point
      pcl::PointXYZRGB point;

      // Color
      point.b = max(0, min(255, (int)(pColor[off3  ]*255)));
      point.g = max(0, min(255, (int)(pColor[off3+1]*255)));
      point.r = max(0, min(255, (int)(pColor[off3+2]*255)));

      // Position
      Eigen::Vector3f pos = K.inverse() * Eigen::Vector3f(x*d, y*d, d);
      point.x = pos[0];
      point.y = pos[1];
      point.z = pos[2];

      pointCloud.push_back(point);
    }
  }

  return pointCloud;
}

void computeSensorOffsetAndK(Eigen::Isometry3f &sensorOffset, Eigen::Matrix3f &cameraMatrix, ParameterCamera *cameraParam, int reduction) {
  sensorOffset = Isometry3f::Identity();
  cameraMatrix.setZero();
      
  int cmax = 4;
  int rmax = 3;
  for (int c=0; c<cmax; c++){
    for (int r=0; r<rmax; r++){
      sensorOffset.matrix()(r, c) = cameraParam->offset()(r, c);
      if (c<3)
	cameraMatrix(r,c) = cameraParam->Kcam()(r, c);
    }
  }
  sensorOffset.translation() = Vector3f(0.15f, 0.0f, 0.05f);
  Quaternionf quat = Quaternionf(0.5, -0.5, 0.5, -0.5);
  sensorOffset.linear() = quat.toRotationMatrix();
  sensorOffset.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f;
	
  float scale = 1./reduction;
  cameraMatrix *= scale;
  cameraMatrix(2,2) = 1;
}

/**
 * ComputeCovarianceMatrix
 */
void ZAdaptiveNormals::computeCovarianceMatrix (const v4r::DataMatrix2D<Eigen::Vector3f> &cloud, const std::vector<int> &indices, const Eigen::Vector3f &mean, Eigen::Matrix3f &cov)
{
  Eigen::Vector3f pt;
  cov.setZero ();

  for (unsigned i = 0; i < indices.size (); ++i)
  {
    pt = cloud.data[indices[i]] - mean;

    cov(1,1) += pt[1] * pt[1];
    cov(1,2) += pt[1] * pt[2];
    cov(2,2) += pt[2] * pt[2];

    pt *= pt[0];
    cov(0,0) += pt[0];
    cov(0,1) += pt[1];
    cov(0,2) += pt[2];
  }

  cov(1,0) = cov(0,1);
  cov(2,0) = cov(0,2);
  cov(2,1) = cov(1,2);
}

/**
 * computeCovarianceMatrix
 */
void PlaneEstimationRANSAC::computeCovarianceMatrix (const std::vector<Eigen::Vector3f> &pts, const std::vector<int> &indices, const Eigen::Vector3f &mean, Eigen::Matrix3f &cov)
{
  Eigen::Vector3f pt;
  cov.setZero ();

  for (unsigned i = 0; i < indices.size (); ++i)
  {
    pt = pts[indices[i]] - mean;

    cov(1,1) += pt[1] * pt[1];
    cov(1,2) += pt[1] * pt[2];

    cov(2,2) += pt[2] * pt[2];

    pt *= pt[0];
    cov(0,0) += pt[0];
    cov(0,1) += pt[1];
    cov(0,2) += pt[2];
  }

  cov(1,0) = cov(0,1);
  cov(2,0) = cov(0,2);
  cov(2,1) = cov(1,2);
}

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

#if 0
  DOF6::TFLinkvf rot1,rot2;
  Eigen::Matrix4f tf1 = build_random_tflink(rot1,3);
  Eigen::Matrix4f tf2 = build_random_tflink(rot2,3);

  DOF6::DOF6_Source<DOF6::TFLinkvf,DOF6::TFLinkvf,float> abc(rot1.makeShared(), rot2.makeShared());
  abc.getRotation();
  abc.getTranslation();

  std::cout<<"tf1\n"<<tf1<<"\n";
  std::cout<<"tf2\n"<<tf2<<"\n";
  std::cout<<"tf1*2\n"<<tf1*tf2<<"\n";
  std::cout<<"tf2*1\n"<<tf2*tf1<<"\n";

  std::cout<<"tf1\n"<<rot1.getTransformation()<<"\n";
  std::cout<<"tf2\n"<<rot2.getTransformation()<<"\n";
  std::cout<<"tf1*2\n"<<(rot1+rot2).getTransformation()<<"\n";

  rot1.check();
  rot2.check();

  return 0;

  pcl::RotationFromCorrespondences rfc;
  Eigen::Vector3f n, nn, v,n2,n3,z,y,tv;
  float a = 0.1f;

  z.fill(0);y.fill(0);
  z(2)=1;y(1)=1;
  nn.fill(0);
  nn(0) = 1;
  Eigen::AngleAxisf aa(a,nn);

  nn.fill(100);

  n.fill(0);
  n(0) = 1;
  n2.fill(0);
  n2=n;
  n2(1) = 0.2;
  n3.fill(0);
  n3=n;
  n3(2) = 0.2;

  n2.normalize();
  n3.normalize();

  tv.fill(1);
  tv.normalize();

#if 0

#if 0
  rfc.add(n,aa.toRotationMatrix()*n+nn,
          1*n.cross(y),1*n.cross(z),
          1*(aa.toRotationMatrix()*n).cross(y),1*(aa.toRotationMatrix()*n).cross(z),
          1,1/sqrtf(3));
#else
  rfc.add(n,aa.toRotationMatrix()*n,
          0*n.cross(y),0*n.cross(z),
          0*(aa.toRotationMatrix()*n).cross(y),0*(aa.toRotationMatrix()*n).cross(z),
          1,0);
#endif

#if 1
  rfc.add(n2,aa.toRotationMatrix()*n2+nn,
          tv,tv,
          tv,tv,
          1,1);
#else
  rfc.add(n2,aa.toRotationMatrix()*n2+nn,
          1*n2.cross(y),1*n2.cross(z),
          1*(aa.toRotationMatrix()*n2).cross(y),1*(aa.toRotationMatrix()*n2).cross(z),
          1,1/sqrtf(3));
#endif

#else
  float f=1;
  Eigen::Vector3f cyl;
  cyl.fill(1);
  cyl(0)=1;
  Eigen::Matrix3f cylM;
  cylM.fill(0);
  cylM.diagonal() = cyl;
  rfc.add(n,aa.toRotationMatrix()*n,
          f*n.cross(y),f*n.cross(z),
          f*(aa.toRotationMatrix()*n).cross(y),f*(aa.toRotationMatrix()*n).cross(z),
          1,0);
  rfc.add(n2,aa.toRotationMatrix()*n2+nn,
          1*n2.cross(y),1*n2.cross(z),
          1*(aa.toRotationMatrix()*n2).cross(y),1*(aa.toRotationMatrix()*n2).cross(z),
          1,1);
#endif
  rfc.add(n3,aa.toRotationMatrix()*n3+nn,
          //tv,tv,
          //tv,tv,
          n3.cross(y),n3.cross(z),
          1*(aa.toRotationMatrix()*n3).cross(y),1*(aa.toRotationMatrix()*n3).cross(z),
//.........这里部分代码省略.........

  void onGMM(const gaussian_mixture_model::GaussianMixture & mix)
  {
    visualization_msgs::MarkerArray msg;
    ROS_INFO("gmm_rviz_converter: Received message.");

    uint i;

    for (i = 0; i < mix.gaussians.size(); i++)
    {
      visualization_msgs::Marker marker;

      marker.header.frame_id = m_frame_id;
      marker.header.stamp = ros::Time::now();
      marker.ns = m_rviz_namespace;
      marker.id = i;
      marker.type = visualization_msgs::Marker::SPHERE;
      marker.action = visualization_msgs::Marker::ADD;
      marker.lifetime = ros::Duration();

      Eigen::Vector3f coords;
      for (uint ir = 0; ir < NUM_OUTPUT_COORDINATES; ir++)
        coords[ir] = m_conversion_mask[ir]->GetMean(m_conversion_mask,mix.gaussians[i]);
      marker.pose.position.x = coords.x();
      marker.pose.position.y = coords.y();
      marker.pose.position.z = coords.z();

      Eigen::Matrix3f covmat;
      for (uint ir = 0; ir < NUM_OUTPUT_COORDINATES; ir++)
        for (uint ic = 0; ic < NUM_OUTPUT_COORDINATES; ic++)
          covmat(ir,ic) = m_conversion_mask[ir]->GetCov(m_conversion_mask,mix.gaussians[i],ic);

      Eigen::EigenSolver<Eigen::Matrix3f> evsolver(covmat);

      Eigen::Matrix3f eigenvectors = evsolver.eigenvectors().real();
      if (eigenvectors.determinant() < 0.0)
        eigenvectors.col(0) = - eigenvectors.col(0);
      Eigen::Matrix3f rotation = eigenvectors;
      Eigen::Quaternionf quat = Eigen::Quaternionf(Eigen::AngleAxisf(rotation));

      marker.pose.orientation.x = quat.x();
      marker.pose.orientation.y = quat.y();
      marker.pose.orientation.z = quat.z();
      marker.pose.orientation.w = quat.w();

      Eigen::Vector3f eigenvalues = evsolver.eigenvalues().real();
      Eigen::Vector3f scale = Eigen::Vector3f(eigenvalues.array().abs().sqrt());
      if (m_normalize)
        scale.normalize();
      marker.scale.x = mix.weights[i] * scale.x() * m_scale;
      marker.scale.y = mix.weights[i] * scale.y() * m_scale;
      marker.scale.z = mix.weights[i] * scale.z() * m_scale;

      marker.color.a = 1.0;
      rainbow(float(i) / float(mix.gaussians.size()),marker.color.r,marker.color.g,marker.color.b);

      msg.markers.push_back(marker);
    }

    // this a waste of resources, but we need to delete old markers in some way
    // someone should add a "clear all" command to rviz
    // (using expiration time is not an option, here)
    for ( ; i < m_max_markers; i++)
    {
      visualization_msgs::Marker marker;
      marker.id = i;
      marker.action = visualization_msgs::Marker::DELETE;
      marker.lifetime = ros::Duration();
      marker.header.frame_id = m_frame_id;
      marker.header.stamp = ros::Time::now();
      marker.ns = m_rviz_namespace;
      msg.markers.push_back(marker);
    }

    m_pub.publish(msg);
    ROS_INFO("gmm_rviz_converter: Sent message.");
  }

osgpcl::PointCloudGeometry* osgpcl::SurfelFactoryFF<PointT, NormalT, RadiusT>::buildGeometry(
        bool unique_state)   {

    typename pcl::PointCloud<NormalT>::ConstPtr normals = this->getInputCloud<NormalT>();
    typename pcl::PointCloud<PointT>::ConstPtr  xyz = this->getInputCloud<PointT>();
    typename pcl::PointCloud<RadiusT>::ConstPtr rads = this->getInputCloud<RadiusT>();

        if (xyz ==NULL) return NULL;
        if (normals ==NULL) return NULL;
        if (rads ==NULL) return NULL;

        if (xyz->points.size() != normals->points.size()) return NULL;
        if (rads->points.size() != normals->points.size()) return NULL;

          pcl::IndicesConstPtr indices = indices_;
        {
            bool rebuild_indices= false;
        if (indices_ == NULL) rebuild_indices=true;
        else if (indices_ ->size() != xyz->points.size() ) rebuild_indices=true;
        if (rebuild_indices){
            pcl::IndicesPtr idxs(new std::vector<int>);
            idxs->reserve(xyz->points.size());
            for(int i=0; i<xyz->points.size(); i++) idxs->push_back(i);
            indices= idxs;
        }
        }

        osg::Vec3Array* pts = new osg::Vec3Array;
        osg::Vec3Array* npts = new osg::Vec3Array;
        int fan_size = 1+ circle_cache.rows();
        pts->reserve(indices->size()*fan_size );
        npts->reserve(indices->size()*fan_size);

        osg::Geometry* geom = new osg::Geometry;

        for(int i=0, pstart=0; i<indices->size(); i++){
            const int& idx = (*indices)[i];
          const PointT& pt =  xyz->points[idx];
          const NormalT& npt = normals->points[idx];
          pts->push_back(osg::Vec3(pt.x, pt.y, pt.z));
          npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z));

          pcl::Normal nt;

          Eigen::Matrix3f rot = Eigen::Matrix3f::Identity();
          rot.row(2) << npt.getNormalVector3fMap().transpose();
          Eigen::Vector3f ax2(1,0,0);
          if ( npt.getNormalVector3fMap().dot(ax2 ) > 0.1) {
              ax2 << 0,1,0;
              if ( npt.getNormalVector3fMap().dot(ax2 ) > 0.1) {
                  ax2 << 0,0,1;
              }
          }
          rot.row(1) << ax2.cross(npt.getNormalVector3fMap()).normalized().transpose();
          rot.row(0) = ( ax2 - ax2.dot(npt.getNormalVector3fMap() )*npt.getNormalVector3fMap() ).normalized();
          rot = rot*rads->points[idx].radius;

          for(int j=0; j<circle_cache.rows(); j++){
              Eigen::Vector3f apt = rot*circle_cache.row(j).transpose() + pt.getVector3fMap();
              pts->push_back(osg::Vec3(apt[0],apt[1],apt[2]));
              npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z));
          }
            geom->addPrimitiveSet( new osg::DrawArrays( GL_TRIANGLE_FAN, pstart,  fan_size ) );
            pstart+= fan_size;
        }
        geom->setVertexArray( pts );
        geom->setNormalArray(npts);
        geom->setNormalBinding( osg::Geometry::BIND_PER_VERTEX );

        geom->setStateSet(stateset_);
        return geom;
}

///
/// @par Detailed description 
/// ... 
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
Eigen::Matrix3f
sasmath::Math::
find_u(sasmol::SasMol &mol, int &frame)
{

    Eigen::Matrix3f r ;

    float rxx = (mol._x().col(frame)*_x().col(frame)).sum() ; 
    float rxy = (mol._x().col(frame)*_y().col(frame)).sum() ;
    float rxz = (mol._x().col(frame)*_z().col(frame)).sum() ;
    
    float ryx = (mol._y().col(frame)*_x().col(frame)).sum() ;
    float ryy = (mol._y().col(frame)*_y().col(frame)).sum() ;
    float ryz = (mol._y().col(frame)*_z().col(frame)).sum() ;

    float rzx = (mol._z().col(frame)*_x().col(frame)).sum() ;
    float rzy = (mol._z().col(frame)*_y().col(frame)).sum() ;
    float rzz = (mol._z().col(frame)*_z().col(frame)).sum() ;

    r << rxx,rxy,rxz,
         ryx,ryy,ryz,
         rzx,rzy,rzz;

    Eigen::Matrix3f rtr = r.transpose() * r ;
    
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(rtr);

    if (eigensolver.info() != Eigen::Success)
    {
          std::cout << "find_u calculation failed" << std::endl ;
    }

    Eigen::Matrix<float,3,1> uk ;  // eigenvalues
    Eigen::Matrix3f ak ;          // eigenvectors

    uk = eigensolver.eigenvalues() ; 
    ak = eigensolver.eigenvectors() ;

    Eigen::Matrix3f akt = ak.transpose() ;
    Eigen::Matrix3f new_ak  ;

    new_ak.row(0) = akt.row(2) ; //sort eigenvectors
    new_ak.row(1) = akt.row(1) ;
    new_ak.row(2) = akt.row(0) ;

    // python ak0 --> ak[2] ; ak1 --> ak[1] ; ak2 --> ak[0]

    //Eigen::Matrix<float,3,1> ak2 = akt.row(2).cross(akt.row(1)) ;

    Eigen::MatrixXf rak0 = (r * (akt.row(2).transpose())) ;
    Eigen::MatrixXf rak1 = (r * (akt.row(1).transpose())) ;

    Eigen::Matrix<float,3,1> urak0 ; 
    if(uk[2] == 0.0)
    { 
        urak0 = 1E15 * rak0 ;
    } 
    else
    {    
        urak0 = (1.0/sqrt(fabs(uk[2])))*rak0 ;
    }
    
    Eigen::Matrix<float,3,1> urak1 ; 
    if(uk[1] == 0.0)
    { 
        urak1 = 1E15 * rak1 ;
    } 
    else
    {    
        urak1 = (1.0/sqrt(fabs(uk[1])))*rak1 ;
    }

    Eigen::Matrix<float,3,1> urak2 = urak0.col(0).cross(urak1.col(0)) ;
    Eigen::Matrix3f bk ;

    bk << urak0,urak1,urak2;

    Eigen::Matrix3f u ;

    u =  (bk * new_ak).transpose() ;

    return u ;
/*
u = array([[-0.94513198,  0.31068658,  0.100992  ],
       [-0.3006246 , -0.70612572, -0.64110165],
       [-0.12786863, -0.63628635,  0.76078203]])
check:

u = 
-0.945133  0.310686  0.100992
-0.300624 -0.706126 -0.641102
-0.127869 -0.636287  0.760782 
*/
}

void
LocalRecognitionPipeline<PointT>::correspondenceGrouping ()
{
    if(cg_algorithm_->getRequiresNormals() && (!scene_normals_ || scene_normals_->points.size() != scene_->points.size()))
        computeNormals<PointT>(scene_, scene_normals_, param_.normal_computation_method_);

    typename std::map<std::string, ObjectHypothesis<PointT> >::iterator it;
    for (it = obj_hypotheses_.begin (); it != obj_hypotheses_.end (); it++)
    {
        ObjectHypothesis<PointT> &oh = it->second;

        if(oh.model_scene_corresp_.size() < 3)
            continue;

        std::vector < pcl::Correspondences > corresp_clusters;
        cg_algorithm_->setSceneCloud (scene_);
        cg_algorithm_->setInputCloud (oh.model_->keypoints_);

        if(cg_algorithm_->getRequiresNormals())
            cg_algorithm_->setInputAndSceneNormals(oh.model_->kp_normals_, scene_normals_);

        //we need to pass the keypoints_pointcloud and the specific object hypothesis
        cg_algorithm_->setModelSceneCorrespondences (oh.model_scene_corresp_);
        cg_algorithm_->cluster (corresp_clusters);

        std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > new_transforms (corresp_clusters.size());
        typename pcl::registration::TransformationEstimationSVD < PointT, PointT > t_est;

        for (size_t i = 0; i < corresp_clusters.size(); i++)
            t_est.estimateRigidTransformation (*oh.model_->keypoints_, *scene_, corresp_clusters[i], new_transforms[i]);

        if(param_.merge_close_hypotheses_) {
            std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > merged_transforms (corresp_clusters.size());
            std::vector<bool> cluster_has_been_taken(corresp_clusters.size(), false);
            const double angle_thresh_rad = param_.merge_close_hypotheses_angle_ * M_PI / 180.f ;

            size_t kept=0;
            for (size_t i = 0; i < new_transforms.size(); i++) {

                if (cluster_has_been_taken[i])
                    continue;

                cluster_has_been_taken[i] = true;
                const Eigen::Vector3f centroid1 = new_transforms[i].block<3, 1> (0, 3);
                const Eigen::Matrix3f rot1 = new_transforms[i].block<3, 3> (0, 0);

                pcl::Correspondences merged_corrs = corresp_clusters[i];

                for(size_t j=i; j < new_transforms.size(); j++) {
                    const Eigen::Vector3f centroid2 = new_transforms[j].block<3, 1> (0, 3);
                    const Eigen::Matrix3f rot2 = new_transforms[j].block<3, 3> (0, 0);
                    const Eigen::Matrix3f rot_diff = rot2 * rot1.transpose();

                    double rotx = atan2(rot_diff(2,1), rot_diff(2,2));
                    double roty = atan2(-rot_diff(2,0), sqrt(rot_diff(2,1) * rot_diff(2,1) + rot_diff(2,2) * rot_diff(2,2)));
                    double rotz = atan2(rot_diff(1,0), rot_diff(0,0));
                    double dist = (centroid1 - centroid2).norm();

                    if ( (dist < param_.merge_close_hypotheses_dist_) && (rotx < angle_thresh_rad) && (roty < angle_thresh_rad) && (rotz < angle_thresh_rad) ) {
                        merged_corrs.insert( merged_corrs.end(), corresp_clusters[j].begin(), corresp_clusters[j].end() );
                        cluster_has_been_taken[j] = true;
                    }
                }

                t_est.estimateRigidTransformation (*oh.model_->keypoints_, *scene_, merged_corrs, merged_transforms[kept]);
                kept++;
            }
            merged_transforms.resize(kept);
            new_transforms = merged_transforms;
        }

        std::cout << "Merged " << corresp_clusters.size() << " clusters into " << new_transforms.size() << " clusters. Total correspondences: " << oh.model_scene_corresp_.size () << " " << it->first << std::endl;

        //        oh.visualize(*scene_);

        size_t existing_hypotheses = models_.size();
        models_.resize( existing_hypotheses + new_transforms.size(), oh.model_  );
        transforms_.insert(transforms_.end(), new_transforms.begin(), new_transforms.end());
    }
}

template<template<class > class Distance, typename PointT, typename FeatureT> bool
GlobalNNPipelineROS<Distance,PointT,FeatureT>::classifyROS (classifier_srv_definitions::classify::Request & req, classifier_srv_definitions::classify::Response & response)
    {
        typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>());
        pcl::fromROSMsg(req.cloud, *cloud);
        this->input_ = cloud;

        pcl::PointCloud<pcl::PointXYZRGB>::Ptr pClusteredPCl (new pcl::PointCloud<pcl::PointXYZRGB>());
        pcl::copyPointCloud(*cloud, *pClusteredPCl);

        //clear all data from previous visualization steps and publish empty markers/point cloud
        for (size_t i=0; i < markerArray_.markers.size(); i++)
        {
            markerArray_.markers[i].action = visualization_msgs::Marker::DELETE;
        }
        vis_pub_.publish( markerArray_ );
        sensor_msgs::PointCloud2 scenePc2;
        vis_pc_pub_.publish(scenePc2);
        markerArray_.markers.clear();

        for(size_t cluster_id=0; cluster_id < req.clusters_indices.size(); cluster_id++)
        {
          std::vector<int> cluster_indices_int;
          Eigen::Vector4f centroid;
          Eigen::Matrix3f covariance_matrix;

          const float r = std::rand() % 255;
          const float g = std::rand() % 255;
          const float b = std::rand() % 255;

          this->indices_.resize(req.clusters_indices[cluster_id].data.size());
          for(size_t kk=0; kk < req.clusters_indices[cluster_id].data.size(); kk++)
          {
                this->indices_[kk] = static_cast<int>(req.clusters_indices[cluster_id].data[kk]);
                pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).r = 0.8*r;
                pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).g = 0.8*g;
                pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).b = 0.8*b;
          }

          this->classify ();

          std::cout << "for cluster " << cluster_id << " with size " << cluster_indices_int.size() << ", I have following hypotheses: " << std::endl;

          object_perception_msgs::classification class_tmp;
          for(size_t kk=0; kk < this->categories_.size(); kk++)
          {
            std::cout << this->categories_[kk] << " with confidence " << this->confidences_[kk] << std::endl;
            std_msgs::String str_tmp;
            str_tmp.data = this->categories_[kk];
            class_tmp.class_type.push_back(str_tmp);
            class_tmp.confidence.push_back( this->confidences_[kk] );
          }
          response.class_results.push_back(class_tmp);

          typename pcl::PointCloud<PointT>::Ptr pClusterPCl_transformed (new pcl::PointCloud<PointT>());
          pcl::computeMeanAndCovarianceMatrix(*this->input_, cluster_indices_int, covariance_matrix, centroid);
          Eigen::Matrix3f eigvects;
          Eigen::Vector3f eigvals;
          pcl::eigen33(covariance_matrix, eigvects,  eigvals);

          Eigen::Vector3f centroid_transformed = eigvects.transpose() * centroid.topRows(3);

          Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Zero(4,4);
          transformation_matrix.block<3,3>(0,0) = eigvects.transpose();
          transformation_matrix.block<3,1>(0,3) = -centroid_transformed;
          transformation_matrix(3,3) = 1;

          pcl::transformPointCloud(*this->input_, cluster_indices_int, *pClusterPCl_transformed, transformation_matrix);

          //pcl::transformPointCloud(*frame_, cluster_indices_int, *frame_eigencoordinates_, eigvects);
          PointT min_pt, max_pt;
          pcl::getMinMax3D(*pClusterPCl_transformed, min_pt, max_pt);
          std::cout << "Elongations along eigenvectors: " << max_pt.x - min_pt.x << ", " << max_pt.y - min_pt.y
                    << ", " << max_pt.z - min_pt.z << std::endl;
          geometry_msgs::Point32 centroid_ros;
          centroid_ros.x = centroid[0];
          centroid_ros.y = centroid[1];
          centroid_ros.z = centroid[2];
          response.centroid.push_back(centroid_ros);

          // calculating the bounding box of the cluster
          Eigen::Vector4f min;
          Eigen::Vector4f max;
          pcl::getMinMax3D (*this->input_, cluster_indices_int, min, max);

          object_perception_msgs::BBox bbox;
          geometry_msgs::Point32 pt;
          pt.x = min[0]; pt.y = min[1]; pt.z = min[2]; bbox.point.push_back(pt);
          pt.x = min[0]; pt.y = min[1]; pt.z = max[2]; bbox.point.push_back(pt);
          pt.x = min[0]; pt.y = max[1]; pt.z = min[2]; bbox.point.push_back(pt);
          pt.x = min[0]; pt.y = max[1]; pt.z = max[2]; bbox.point.push_back(pt);
          pt.x = max[0]; pt.y = min[1]; pt.z = min[2]; bbox.point.push_back(pt);
          pt.x = max[0]; pt.y = min[1]; pt.z = max[2]; bbox.point.push_back(pt);
          pt.x = max[0]; pt.y = max[1]; pt.z = min[2]; bbox.point.push_back(pt);
          pt.x = max[0]; pt.y = max[1]; pt.z = max[2]; bbox.point.push_back(pt);
          response.bbox.push_back(bbox);

          // getting the point cloud of the cluster
          typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
          pcl::copyPointCloud(*this->input_, cluster_indices_int, *cluster);
//.........这里部分代码省略.........

 )
{
  Approx approx = Approx::custom().epsilon( 0.00001 );
  
  OSinput oi = OSinput(pdg2014::Mb, 80.399, 91.1876, 125.6, 173.5);
    
  SECTION( "MS mass from OS one,\\mu=M" ) 
    {

      // We use b-quark due to nl=2*NL
      bb<OS> dMt(oi, oi.MMb());

      SECTION( "Three-loop zm3 = c0 + c1*nl + c2*nl^2" )
        {
          
          Eigen::Matrix3f mM3;
          Eigen::Vector3f x3NL;
          
          x3NL(0) = dMt.x03(0,1);
          x3NL(1) = dMt.x03(1,1);
          x3NL(2) = dMt.x03(2,1);
          
          // c0 + c1*nl + c2*nl^2
          mM3 <<
            1, 0, 0,
            1, 2, 4,
            1, 4, 16;
          
          Eigen::Vector3f ci3 = pow(4,-3)*mM3.colPivHouseholderQr().solve(x3NL);
          
          // NL^0