C++ Quaternionf::normalize方法代码示例

TEST (PCL, WarpPointRigid6DFloat)
{
  WarpPointRigid6D<PointXYZ, PointXYZ, float> warp;
  Eigen::Quaternionf q (0.4455f, 0.9217f, 0.3382f, 0.3656f);
  q.normalize ();
  Eigen::Vector3f t (0.82550f, 0.11697f, 0.44864f);

  Eigen::VectorXf p (6);
  p[0] = t.x (); p[1] = t.y (); p[2] = t.z (); p[3] = q.x (); p[4] = q.y (); p[5] = q.z ();
  warp.setParam (p);

  PointXYZ pin (1, 2, 3), pout;
  warp.warpPoint (pin, pout);
  EXPECT_NEAR (pout.x, 4.15963f, 1e-5);
  EXPECT_NEAR (pout.y, -1.51363f, 1e-5);
  EXPECT_NEAR (pout.z, 0.922648f, 1e-5);
}

bool
psmove_alignment_quaternion_between_vector_frames(
	const Eigen::Vector3f* from[2], const Eigen::Vector3f* to[2], const float tolerance, const Eigen::Quaternionf &initial_q,
	Eigen::Quaternionf &out_q)
{
	bool success = true;

	Eigen::Quaternionf previous_q = initial_q;
	Eigen::Quaternionf q = initial_q;
	
    //const float tolerance_squared = tolerance*tolerance; //TODO: This variable is unused, but it should be. Need to re-test with this added since the below test should be: error_squared > tolerance_squared
	const int k_max_iterations = 32;
	float previous_error_squared = k_real_max;
	float error_squared = k_real_max;
	float squared_error_delta = k_real_max;
	float gamma = 0.5f;
	bool backtracked = false;

	for (int iteration = 0; 
		iteration < k_max_iterations && // Haven't exceeded iteration limit
		error_squared > tolerance && // Aren't within tolerance of the destination
		squared_error_delta > k_normal_epsilon && // Haven't reached a minima
		gamma > k_normal_epsilon; // Haven't reduced our step size to zero
		iteration++)
	{
		// Fill in the 6x1 objective function matrix |f_0|
		//                                           |f_1|
		float error_squared0, error_squared1;

		Eigen::Matrix<float, 3, 1> f_0;
		psmove_alignment_compute_objective_vector(q, *from[0], *to[0], f_0, &error_squared0);

		Eigen::Matrix<float, 3, 1> f_1;
		psmove_alignment_compute_objective_vector(q, *from[1], *to[1], f_1, &error_squared1);

		Eigen::Matrix<float, 6, 1> f;
		f.block<3, 1>(0, 0) = f_0;
		f.block<3, 1>(3, 0) = f_1;

		error_squared = error_squared0 + error_squared1;

		// Make sure this new step hasn't made the error worse
		if (error_squared <= previous_error_squared)
		{
			// We won't have a valid error derivative if we had to back track
			squared_error_delta = !backtracked ? fabsf(error_squared - previous_error_squared) : squared_error_delta;
			backtracked = false;

			// This is a good step.
			// Remember it in case the next one makes things worse
			previous_error_squared = error_squared;
			previous_q = q;

			// Fill in the 4x6 objective function Jacobian matrix: [J_0|J_1]
			Eigen::Matrix<float, 4, 3> J_0;
			psmove_alignment_compute_objective_jacobian(q, *from[0], J_0);

			Eigen::Matrix<float, 4, 3> J_1;
			psmove_alignment_compute_objective_jacobian(q, *from[1], J_1);

			Eigen::Matrix<float, 4, 6> J;
			J.block<4, 3>(0, 0) = J_0; J.block<4, 3>(0, 3) = J_1;

			// Compute the gradient of the objective function
			Eigen::Matrix<float, 4, 1> gradient_f = J*f;
			Eigen::Quaternionf gradient_q =
				Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0));

			// The gradient points toward the maximum, so we subtract it off to head toward the minimum.
			// The step scale 'gamma' is just a guess.			
			q = Eigen::Quaternionf(q.coeffs() - gradient_q.coeffs()*gamma); //q-= gradient_q*gamma;
			q.normalize();
		}
		else
		{
			// The step made the error worse.
			// Return to the previous orientation and half our step size.
			q = previous_q;
			gamma /= 2.f;
			backtracked = true;
		}
	}

	if (error_squared > tolerance)
	{
		// Make sure we didn't fail to converge on the goal
		success = false;
	}

	out_q= q;

	return success;
}

/**
 * @brief Callback for feedback subscriber for getting the transformation of moved markers
 *
 * @param feedback subscribed from geometry_map/map/feedback
 */
void ShapeVisualization::setShapePosition(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)//,const cob_3d_mapping_msgs::Shape& shape)
{

  cob_3d_mapping_msgs::ShapeArray map_msg;
  map_msg.header.frame_id="/map";
  map_msg.header.stamp = ros::Time::now();

  int shape_id,index;
  index=-1;
  stringstream name(feedback->marker_name);

  Eigen::Quaternionf quat;

  Eigen::Matrix3f rotationMat;
  Eigen::MatrixXf rotationMatInit;

  Eigen::Vector3f normalVec;
  Eigen::Vector3f normalVecNew;
  Eigen::Vector3f newCentroid;
  Eigen::Matrix4f transSecondStep;


  if (feedback->marker_name != "Text"){
    name >> shape_id ;
    cob_3d_mapping::Polygon p;

    for(int i=0;i<sha.shapes.size();++i)
    {
    	if (sha.shapes[i].id == shape_id)
	{
		index = i;
	}

    }


    // temporary fix.
    //do nothing if index of shape is not found
    // this is not supposed to occur , but apparently it does
    if(index==-1){

    ROS_WARN("shape not in map array");
    return;
	}


    cob_3d_mapping::fromROSMsg (sha.shapes.at(index), p);

    if (feedback->event_type == 2 && feedback->menu_entry_id == 5){
      quatInit.x() = (float)feedback->pose.orientation.x ;           //normalized
      quatInit.y() = (float)feedback->pose.orientation.y ;
      quatInit.z() = (float)feedback->pose.orientation.z ;
      quatInit.w() = (float)feedback->pose.orientation.w ;

      oldCentroid (0) = (float)feedback->pose.position.x ;
      oldCentroid (1) = (float)feedback->pose.position.y ;
      oldCentroid (2) = (float)feedback->pose.position.z ;

      quatInit.normalize() ;

      rotationMatInit = quatInit.toRotationMatrix() ;

      transInit.block(0,0,3,3) << rotationMatInit ;
      transInit.col(3).head(3) << oldCentroid(0) , oldCentroid(1), oldCentroid(2) ;
      transInit.row(3) << 0,0,0,1 ;

      transInitInv = transInit.inverse() ;
      Eigen::Affine3f affineInitFinal (transInitInv) ;
      affineInit = affineInitFinal ;

      std::cout << "transInit : " << "\n"    << affineInitFinal.matrix() << "\n" ;
    }

    if (feedback->event_type == 5){
      /* the name of the marker is arrows_shape_.id, we need to erase the "arrows_" part */
      //string strName(feedback->marker_name);
      //strName.erase(strName.begin(),strName.begin()+7);
//      stringstream name(strName);
	stringstream name(feedback->marker_name);

      name >> shape_id ;
      cob_3d_mapping::Polygon p;
      cob_3d_mapping::fromROSMsg (sha.shapes.at(index), p);

      quat.x() = (float)feedback->pose.orientation.x ;           //normalized
      quat.y() = (float)feedback->pose.orientation.y ;
      quat.z() = (float)feedback->pose.orientation.z ;
      quat.w() = (float)feedback->pose.orientation.w ;

      quat.normalize() ;

      rotationMat = quat.toRotationMatrix() ;

      normalVec << sha.shapes.at(index).params[0],                   //normalized
          sha.shapes.at(index).params[1],
//.........这里部分代码省略.........

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

    parseCommandLine(argc, argv);

    /* CARICO GLI ARGOMENTI*/




    /* CREO L'OGGETTO CHE CALCOLA LE CORRISPONDENZE*/
    Correspondance corr;

    /* LEGGO LE POINT CLOUD*/
    if (pcl::io::loadPCDFile<point> (modstr, *(corr.Modello)) == -1) //* load the file
    {
        PCL_ERROR("Couldn't read file \n");
        return (-1);
    }




    if (pcl::io::loadPCDFile<point> (scestr, *(corr.Scena)) == -1) //* load the file
    {
        PCL_ERROR("Couldn't read file \n");
        return (-1);
    }



    if (transformscene) {

        Eigen::Affine3f T;
        Eigen::Quaternionf q = Eigen::Quaternionf(qw, qx, qy, qz);
        q.normalize();
        T = Eigen::Translation<float, 3>(tx, ty, tz) * q;

        CloudT old(new pcl::PointCloud<point>);
        pcl::copyPointCloud(*corr.Scena, *old);
        pcl::transformPointCloud(*corr.Scena, *corr.Scena, T);
        pcl::visualization::PointCloudColorHandlerCustom<point> white(old, 255, 255, 255);
        pcl::visualization::PointCloudColorHandlerCustom<point> green(corr.Scena, 0, 255, 0);
        pcl::visualization::PCLVisualizer tv;
        tv.addPointCloud<point>(old, white, "old");
        tv.addPointCloud<point>(corr.Scena, green, "new");

        while (!tv.wasStopped()) {
            tv.spinOnce(100);
        }

        tv.setSize(1, 1);

    }
    /* VISUALIZZO LE POINTCLOUD*/



    pcl::visualization::PCLVisualizer::Ptr v(new pcl::visualization::PCLVisualizer);
    pcl::visualization::PCLVisualizer::Ptr w(new pcl::visualization::PCLVisualizer);

    pcl::visualization::PointCloudColorHandlerCustom<point> whiteM(corr.Modello, 255, 255, 255);
    pcl::visualization::PointCloudColorHandlerCustom<point> whiteS(corr.Scena, 255, 255, 255);


    v->addPointCloud<point>(corr.Modello, whiteM, "modello");
    v->setPosition(10, 10);
    v->setSize(640, 480);
    w->addPointCloud<point>(corr.Scena, whiteS, "scena");
    w->setPosition(650, 10);
    w->setSize(640, 480);
    v->setWindowName("MODELLO");
    w->setWindowName("SCENA");

    while (!(v->wasStopped())&&!(w->wasStopped())) {
        v->spinOnce(10);
        w->spinOnce(10);

    };
    w->setSize(1, 1);
    v->setSize(1, 1);




    corr.kmatch = kmatchglobal;
    corr.HistDistTreshSquare = HistDistTreshSquare;
    corr.DescrizioneModello.name="Modello";
    corr.DescrizioneScena.name="Scene";
    
    /* FILTRAGGIO E ELIMINAZIONE DEL PIANO */
    corr.DescrizioneModello.ep.rimuovipiano = modelremoveplane;
    corr.DescrizioneModello.ep.percent = PlanePercent;
    corr.DescrizioneModello.ep.distance_threshold = distance_treshold;
    corr.DescrizioneModello.ep.max_iterations = PlaneMaxIter;
    corr.DescrizioneModello.ep.Rfilter = Rfilter;

    corr.DescrizioneScena.ep.rimuovipiano =sceneremoveplane;
    corr.DescrizioneScena.ep.percent = PlanePercent;
    corr.DescrizioneScena.ep.distance_threshold = distance_treshold;
    corr.DescrizioneScena.ep.max_iterations = PlaneMaxIter;
//.........这里部分代码省略.........