C++ Matrix::norm方法代码示例

    inline static void setFromVectors(Rotation_& rot, const Eigen::Matrix<PrimType_, 3, 1>& v1, const Eigen::Matrix<PrimType_, 3, 1>& v2) {
        KINDR_ASSERT_TRUE(std::runtime_error,  v1.norm()*v2.norm() != static_cast<PrimType_>(0.0), "At least one vector has zero length.");

        Eigen::Quaternion<PrimType_> eigenQuat;
        eigenQuat.setFromTwoVectors(v1, v2);
        rot = kindr::rotations::eigen_impl::RotationQuaternion<PrimType_, Usage_>(eigenQuat);
//
//    const PrimType_ angle = acos(v1.dot(v2)/temp);
//    const PrimType_ tol = 1e-3;
//
//    if(0 <= angle && angle < tol) {
//      rot.setIdentity();
//    } else if(M_PI - tol < angle && angle < M_PI + tol) {
//      rot = eigen_impl::AngleAxis<PrimType_, Usage_>(angle, 1, 0, 0);
//    } else {
//      const Eigen::Matrix<PrimType_, 3, 1> axis = (v1.cross(v2)).normalized();
//      rot = eigen_impl::AngleAxis<PrimType_, Usage_>(angle, axis);
//    }
    }

Quaternion<FloatT> exp(Eigen::Matrix<FloatT, 3, 1> v)
{
	FloatT angle = v.norm();
	if (angle <= Eigen::machine_epsilon<FloatT>()) {
		// std::cerr << "Warning: tiny quaternion flushed to zero\n";
		return Quaternion<FloatT>::Identity();
	}
	else {
		Quaternion<FloatT> ret;
#if 0
		if (angle > 1.999*M_PI) {
			// TODO: I really, really don't like this hack. It should
			// be impossible to compute an angular measurement update
			// with a rotation angle greater than this number...
			v *= 1.999*M_PI / angle;
			angle = 1.999*M_PI;
		}
#endif
		assert(angle <= FloatT(2.0*M_PI));
#if 0
		// Oddly enough, this attempt to make the formula faster by reducing
		// the number of trig calls actually runs slower.
		FloatT tan_x = std::tan(angle * 0.25);
		FloatT cos_angle = (1 - tan_x*tan_x)/(1+tan_x*tan_x);
		FloatT sin_angle = 2*tan_x/(1+tan_x*tan_x);
		ret.w() = cos_angle;
		ret.vec() = (sin_angle/angle)*v;
#else
		ret.w() = std::cos(angle*0.5);
		ret.vec() = (std::sin(angle*0.5)/angle)*v;
#endif
		return ret;
		// return Quaternion<FloatT>(Eigen::AngleAxis<FloatT>(angle, v / angle));
	}
}

 ReferenceSO3(const Eigen::Vector3d &v) : AbstractTestSO3(v) {
   Eigen::Matrix<float_100, 3, 1> axis = v.cast<float_100>();
   float_100 angle = axis.norm();
   if (angle > 0) {
     axis /= angle;
   }
   m_aa = Eigen::AngleAxis<float_100>(angle, axis);
 }

void OpenGL3DSphereRenderer::saveTriangleInMem( const Eigen::Matrix<GLfloat,3,1>& v1, const Eigen::Matrix<GLfloat,3,1>& v2, const Eigen::Matrix<GLfloat,3,1>& v3, unsigned& current_vertex )
{
  assert( fabs( double(v1.norm() - 1.0f) ) <= 1.0e-6 );
  m_sphere_verts.col( current_vertex ) = v1;
  m_sphere_normals.col( current_vertex ) = v1;
  ++current_vertex;

  assert( fabs( double(v2.norm() - 1.0f) ) <= 1.0e-6 );
  m_sphere_verts.col( current_vertex ) = v2;
  m_sphere_normals.col( current_vertex ) = v2;
  ++current_vertex;

  assert( fabs( double(v3.norm() - 1.0f) ) <= 1.0e-6 );
  m_sphere_verts.col( current_vertex ) = v3;
  m_sphere_normals.col( current_vertex ) = v3;
  ++current_vertex;
}

 Eigen::Matrix<double,N,1> normalizeAndJacobian(const Eigen::Matrix<double,N,1> & v, Eigen::Matrix<double,N,N> & outJacobian)
 {
   double recip_s_vtv = 1.0/v.norm();
   
   outJacobian = (Eigen::Matrix<double,N,N>::Identity() * recip_s_vtv) - (recip_s_vtv*recip_s_vtv*recip_s_vtv)*v*v.transpose();
   
   return v*recip_s_vtv;
 }

///
/// @par Detailed description 
/// ... 
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
float
sasmath::
calc_angle(Eigen::Matrix<float,3,1> &coor1, Eigen::Matrix<float,3,1> &coor2, Eigen::Matrix<float,3,1> &coor3)
{
    /*
    Method to calculate the angle between three atoms
    */

    float angle ;

    Eigen::Matrix<float,3,1> u ; u = coor1 - coor2 ;
    Eigen::Matrix<float,3,1> v ; v = coor3 - coor2 ;

    float c = (u.dot(v))/(u.norm()*v.norm()) ;
    angle = acos(c) ;

    return angle ;
}

TEST_F(MaterialLibSolidsKelvinVector6, DeviatoricSphericalProjections)
{
    auto const& P_dev = Invariants<size>::deviatoric_projection;
    auto const& P_sph = Invariants<size>::spherical_projection;

    // Test product P_dev * P_sph is zero.
    Eigen::Matrix<double, 6, 6> const P_dev_P_sph = P_dev * P_sph;
    EXPECT_LT(P_dev_P_sph.norm(), std::numeric_limits<double>::epsilon());
    EXPECT_LT(P_dev_P_sph.maxCoeff(), std::numeric_limits<double>::epsilon());

    // Test product P_sph * P_dev is zero.
    Eigen::Matrix<double, 6, 6> const P_sph_P_dev = P_sph * P_dev;
    EXPECT_LT(P_sph_P_dev.norm(), std::numeric_limits<double>::epsilon());
    EXPECT_LT(P_sph_P_dev.maxCoeff(), std::numeric_limits<double>::epsilon());

    // Test sum is identity.
    EXPECT_EQ(P_dev + P_sph, (Eigen::Matrix<double, size, size>::Identity()));
}

Eigen::Matrix<double, 3, 3> getRotationFromA2B(Eigen::Matrix<double, 3, 1> A, Eigen::Matrix<double, 3, 1> B) {
	Eigen::Matrix<double, 3, 1> v = A.cross(B);
	double s = v.norm();
	double c = A.dot(B);

	Eigen::Matrix<double, 3, 3> vx;
	vx <<   0, -v[2], v[1],
			v[2], 0, -v[0],
			-v[1], v[0], 0;
	return Eigen::Matrix<double, 3, 3>::Identity() + vx + ((1.0-c)/(s*s))*vx*vx;
}

void singleArmTranslationConstraint:: constraintUpdate(const Eigen::Matrix<double, 3, 1> &currentTranslation,
						       const Eigen::Matrix<double, 3, 1> &desiredTranslation,
						       const Eigen::Matrix<double, 3, 1> &timeDerivative,
						       const Eigen::Matrix<double, 6, DOF> &gradient
						       ){

  // update function measure
  Eigen::Matrix<double, 3, 1> measure;
  measure.setZero();
  measure = currentTranslation - desiredTranslation;
  


  measureNorm_ = measure.norm();

  // update the constraint gradient
  Eigen::Matrix<double, 3, DOF> tempGradient;
  tempGradient.setZero();
  tempGradient = gradient.block<3,DOF>(0,0);
    
  // set up the gradient
  for(int j = 0; j<dim_; j++){
    for(int i = 0; i<DOF; i++){
      // int k = i + sIndex_;
      modelPointer_->chgCoeff(
			      *constrPArray_[j],
			      *(*jointVVarPArrayPointer_)[i],
			      tempGradient(j, i)
			      // gradient(j, i)
			      );	
    }// finish one constraint
  }// finish all constraints


  Eigen::Matrix<double, 3, 1> rhs;
  rhs.setZero();
  // update the rhs
  for(int j = 0; j<dim_; j++){
    
    rhs(j) =  - gain_*( currentTranslation(j) - desiredTranslation(j) ); 

    constrPArray_[j]->set(GRB_DoubleAttr_RHS, 
			  rhs(j)
			  );

  }

  // std::cout<<"error   X: "<<measure(0)           <<" error   Y: "<<measure(1)           <<" error   Z: "<<measure(2)<<std::endl;
  // std::cout<<"error   norm: "<<measureNorm_<<std::endl;
  // std::cout<<"seperation line:---------------------------- "<<std::endl;

}

    static bool testApply()
    {
        bool b, ret;
        // apply delta:
        Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix<double, 4, 4> expectedT = Eigen::Matrix<double, 4, 4>::Identity();
        Eigen::Matrix<double, 4, 4> diff;

        SE3<double> pose;
        pose.set( delta );
        delta[ 0 ] = Math::deg2Rad( 1.5 );
        delta[ 1 ] = Math::deg2Rad( 1.1 );
        delta[ 2 ] = Math::deg2Rad( 1.6 );
        delta[ 3 ] = 1;
        delta[ 4 ] = 1;
        delta[ 5 ] = 1;
        pose.apply( delta );

        expectedT( 0, 3 ) = delta[ 3 ];
        expectedT( 1, 3 ) = delta[ 4 ];
        expectedT( 2, 3 ) = delta[ 5 ];

        Eigen::Matrix<double, 3, 1> axis = delta.segment<3>( 0 );
        double angle = axis.norm();	axis /= angle;

        expectedT.block<3, 3>( 0, 0 ) = Eigen::AngleAxis<double>( angle, axis ).toRotationMatrix();
        diff = expectedT - pose.transformation();

        ret = b = ( diff.array().abs().sum() / 12 < 0.001 );

        if( !b ){
            std::cout << expectedT << std::endl;
            std::cout << pose.transformation() << std::endl;
            std::cout << "avg SAD: " << diff.array().abs().sum() / 12 << std::endl;
        }

        pose.apply( -delta );
        expectedT.setIdentity();

        b &= ( ( expectedT - pose.transformation() ).array().abs().sum() / 12 < 0.0001 );
        CVTTEST_PRINT( "apply", b );
        ret &= b;

        return ret;
    }

 static typename math::scalar_of<T>::type get(const Eigen::Matrix<T, N, M> &x) {
     return x.norm();
 }

double SolveODE(
		std::list<double> &list_val,
		std::list<Eigen::Matrix<double,Eigen::Dynamic,1> > &list_pos,
		std::list<Eigen::Matrix<double,Eigen::Dynamic,1> > &list_vit,
		std::list<Eigen::Matrix<double,Eigen::Dynamic,1> > &list_grad,
		double &t_tot,
		double &v_tot,
		const std::vector<skeleton::BranchContProjSkel::Ptr> &skel,
		const Eigen::Matrix<double,Eigen::Dynamic,1> &q_init,
		const Eigen::Matrix<double,Eigen::Dynamic,1> &v_init,
		const double &lambda,
		const double &deltat)
{
	double value = 0.0;
	double valuef = 0.0;
	Eigen::Matrix<double,Eigen::Dynamic,1> gradient(q_init.rows(),1);
	Eigen::Matrix<double,Eigen::Dynamic,1> gradientf(q_init.rows(),1);
	v_tot = 0.0;
	t_tot = 0.0;

	Dist_grad(q_init,skel,valuef,gradientf);

	list_pos.push_back(q_init);
	list_vit.push_back(v_init);
	list_grad.push_back(gradientf);
	list_val.push_back(valuef);


	//first step, to avoid boundary problem :
	Eigen::Matrix<double,Eigen::Dynamic,1> q = q_init + v_init * deltat;
	Eigen::Matrix<double,Eigen::Dynamic,1> v = v_init;

	gradient = gradientf;
	value    = valuef;
	Dist_grad(q,skel,valuef,gradientf);

	while(!Oob(q) && t_tot<10.0)
	{
		list_pos.push_back(q);
		list_vit.push_back(v);
		list_grad.push_back(gradientf);
		list_val.push_back(valuef);

		value = valuef;
		gradient = gradientf;
		Dist_grad(q,skel,valuef,gradientf);

		double vnor = v.norm();

		double frac = deltat/vnor;
		v_tot+=(vnor*lambda + (value+valuef)/2.0)*frac;
		t_tot += frac;

		Eigen::Matrix<double,Eigen::Dynamic,1> acc = gradientf*(1.0/lambda);

		q = q + v*frac;
		v = v + acc*frac;
	}


	Eigen::Matrix<double,Eigen::Dynamic,1> qf = Eigen::Matrix<double,Eigen::Dynamic,1>::Ones(q.rows(),1);
	value = valuef;
	gradient = gradientf;
	Dist_grad(qf,skel,valuef,gradientf);

	double df = (q-qf).norm();

	v_tot+=df*lambda + ((valuef+value)/2.0)*df;

	list_pos.push_back(qf);
	list_vit.push_back(v);
	list_grad.push_back(gradientf);
	list_val.push_back(valuef);

	return df;
}

 Eigen::Matrix<double,N,1> normalize(const Eigen::Matrix<double,N,1> & v)
 {
   return v/v.norm();
 }