C++ Matrix3f类代码示例

int main(int, char**)
{
  cout.precision(3);
  Matrix3f m;
m.row(0) << 1, 2, 3;
m.block(1,0,2,2) << 4, 5, 7, 8;
m.col(2).tail(2) << 6, 9;		    
std::cout << m;

  return 0;
}

// setup _Bearth
void Compass::_setup_earth_field(void)
{
    // assume a earth field strength of 400
    _hil.Bearth(400, 0, 0);
    
    // rotate _Bearth for inclination and declination. -66 degrees
    // is the inclination in Canberra, Australia
    Matrix3f R;
    R.from_euler(0, ToRad(66), get_declination());
    _hil.Bearth = R * _hil.Bearth;
}

void Camera::SetView(const Vector3f &position, const Vector3f &target,
                     const Vector3f &up) {
  Matrix3f rotation;
  rotation.row(1) = up.normalized();
  rotation.row(2) = (position - target).normalized();
  rotation.row(0) = rotation.row(1).cross(rotation.row(2));
  view_.topLeftCorner<3, 3>() = rotation;
  view_.topRightCorner<3, 1>() = -rotation * position;
  view_(3, 3) = 1.0;
  matrix_dirty_ = true;
}

int main(int, char**)
{
  cout.precision(3);
  Matrix3f m;
m = AngleAxisf(0.25*M_PI, Vector3f::UnitX())
  * AngleAxisf(0.5*M_PI,  Vector3f::UnitY())
  * AngleAxisf(0.33*M_PI, Vector3f::UnitZ());
cout << m << endl << "is unitary: " << m.isUnitary() << endl;

  return 0;
}

int main()
{
   Matrix3f A;
   Vector3f b;
   A << 1,2,3,  4,5,6,  7,8,10;
   b << 3, 3, 4;
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "Here is the vector b:\n" << b << endl;
   Vector3f x = A.colPivHouseholderQr().solve(b);
   cout << "The solution is:\n" << x << endl;
}

void RealtimeMF_openni::normals_cb(float* d_normals, uint8_t* haveData,
    uint32_t w, uint32_t h)
{
  tLog_.tic(-1); // reset all timers
  int32_t nComp = 0;
  float* d_nComp = this->normalExtract->d_normalsComp(nComp);
  Matrix3f kRwBefore = kRw_;
  tLog_.toctic(0,1);

  optSO3_->updateExternalGpuNormals(d_nComp,nComp,3,0);
  double residual = optSO3_->conjugateGradientCUDA(kRw_,nCGIter_);
  double D_KL = optSO3_->D_KL_axisUnif();
  tLog_.toctic(1,2);

  {
    boost::mutex::scoped_lock updateLock(this->updateModelMutex);
    this->normalsImg_ = this->normalExtract->normalsImg();
    if(z_.rows() != w*h) z_.resize(w*h);
    this->normalExtract->uncompressCpu(optSO3_->z().data(), optSO3_->z().rows(),
        z_.data(), z_.rows());

    mfAxes_ = MatrixXf::Zero(3,6);
    for(uint32_t k=0; k<6; ++k){
      int j = k/2; // which of the rotation columns does this belong to
      float sign = (- float(k%2) +0.5f)*2.0f; // sign of the axis
      mfAxes_.col(k) = sign*kRw_.col(j);
    }
    D_KL_= D_KL;
    residual_ = residual;
    this->update_ = true;
    updateLock.unlock();
  }

  tLog_.toc(2); // total time
  tLog_.logCycle();
  cout<<"delta rotation kRw_ = \n"<<kRwBefore*kRw_.transpose()<<endl;
  cout<<"---------------------------------------------------------------------------"<<endl;
  tLog_.printStats();
  cout<<" residual="<<residual_<<"\t D_KL="<<D_KL_<<endl;
  cout<<"---------------------------------------------------------------------------"<<endl;
  fout_<<D_KL_<<" "<<residual_<<endl; fout_.flush();

////  return kRw_;
//  {
//    boost::mutex::scoped_lock updateLock(this->updateModelMutex);
////    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr nDispPtr =
////      normalExtract->normalsPc();
////    nDisp_ = pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new
////        pcl::PointCloud<pcl::PointXYZRGB>(*nDispPtr));
////    this->normalsImg_ = this->normalExtract->normalsImg();
//    this->update_ = true;
//  }
};

// static
SimilarityTransform SimilarityTransform::fromMatrix( const Matrix4f& m )
{
    Matrix3f r = m.getSubmatrix3x3();
    float s = r.getRow( 0 ).norm();

    return
    {
        s,
        r / s,
        m.getCol( 3 ).xyz
    };
}

Eigen::Matrix4f ConsistencyTest::initPose2D( std::map<unsigned, unsigned> &matched_planes )
{
  //Calculate rotation
  Matrix3f normalCovariances = Matrix3f::Zero();
  for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it != matched_planes.end(); it++)
    normalCovariances += PBMTarget.vPlanes[it->second].v3normal * PBMSource.vPlanes[it->first].v3normal.transpose();
  normalCovariances(1,1) += 100; // Rotation "restricted" to the y axis

  JacobiSVD<MatrixXf> svd(normalCovariances, ComputeThinU | ComputeThinV);
  Matrix3f Rotation = svd.matrixU() * svd.matrixV().transpose();

  if(Rotation.determinant() < 0)
//    Rotation.row(2) *= -1;
    Rotation = -Rotation;

  // Calculate translation
  Vector3f translation;
  Vector3f center_data = Vector3f::Zero(), center_model = Vector3f::Zero();
  Vector3f centerFull_data = Vector3f::Zero(), centerFull_model = Vector3f::Zero();
  unsigned numFull = 0, numNonStruct = 0;
  for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it != matched_planes.end(); it++)
  {
    if(PBMSource.vPlanes[it->first].bFromStructure) // The certainty in center of structural planes is too low
      continue;

    ++numNonStruct;
    center_data += PBMSource.vPlanes[it->first].v3center;
    center_model += PBMTarget.vPlanes[it->second].v3center;
    if(PBMSource.vPlanes[it->first].bFullExtent)
    {
      centerFull_data += PBMSource.vPlanes[it->first].v3center;
      centerFull_model += PBMTarget.vPlanes[it->second].v3center;
      ++numFull;
    }
  }
  if(numFull > 0)
  {
    translation = (-centerFull_model + Rotation * centerFull_data) / numFull;
  }
  else
  {
    translation = (-center_model + Rotation * center_data) / numNonStruct;
  }

  translation[1] = 0; // Restrict no translation in the y axis

  // Form SE3 transformation matrix. This matrix maps the model into the current data reference frame
  Eigen::Matrix4f rigidTransf;
  rigidTransf.block(0,0,3,3) = Rotation;
  rigidTransf.block(0,3,3,1) = translation;
  rigidTransf.row(3) << 0,0,0,1;
  return rigidTransf;
}

int main(int, char**)
{
  cout.precision(3);
  Matrix3f A;
Vector3f b;
A << 1,2,3,  4,5,6,  7,8,10;
b << 3, 3, 4;
Vector3f x = A.inverse() * b;
cout << "The solution is:" << endl << x << endl;

  return 0;
}

void ExtendedWmlCamera::OrbitVertical( float fAngle )
{
	Matrix3f rotate;
	rotate.FromAxisAngle( GetLeft(), fAngle );
	
	Vector3f direction( GetLocation() - m_ptTarget );
	Vector3f newUp( rotate * GetUp() );
	newUp.Normalize();

	SetTargetFrame( m_ptTarget + (rotate * direction), GetLeft(), 
		newUp, m_ptTarget );
}

/* evaluate cost function for a given assignment of npormals to axes */
float mmf::OptSO3vMFCF::evalCostFunction(Matrix3f& R)
{
  float c = 0.0f;
  for (uint32_t j=0; j<6; ++j) { 
    if(j%2 ==0){
      c -= this->cld_.xSums().col(j).transpose()*R.col(j/2);
    }else{
      c += this->cld_.xSums().col(j).transpose()*R.col(j/2);
    }
  }
  return c;
}

//--------------------------------------------------------------------------------
void App::Update()
{
	// Update the timer to determine the elapsed time since last frame.  This can 
	// then used for animation during the frame.

	m_pTimer->Update();

	// Send an event to everyone that a new frame has started.  This will be used
	// in later examples for using the material system with render views.

	EvtManager.ProcessEvent( EvtFrameStartPtr( new EvtFrameStart( m_pTimer->Elapsed() ) ) );


	// Manipulate the scene here - simply rotate the root of the scene in this
	// example.

	Matrix3f rotation;
	rotation.RotationY( m_pTimer->Elapsed() );
	m_pActor->GetNode()->Transform.Rotation() *= rotation;


	// Update the scene, and then render all cameras within the scene.

	m_pScene->Update( m_pTimer->Elapsed() );
	m_pScene->Render( m_pRenderer11 );


	// Perform the rendering and presentation for each window.

	for ( int i = 0; i < NUM_WINDOWS; i++ )
	{
		// Bind the swap chain render target and the depth buffer for use in 
		// rendering.  

		D3D11_BOX box;
		box.left = m_pWindow[i]->GetLeft();
		box.right = m_pWindow[i]->GetWidth() + box.left;
		box.top = m_pWindow[i]->GetTop();
		box.bottom = m_pWindow[i]->GetHeight() + box.top;
		box.front = 0;
		box.back = 1;

		if ( box.left < 0 ) box.left = 0;
		if ( box.right > (unsigned int)m_DesktopRes.x - 1 ) box.right = (unsigned int)m_DesktopRes.x - 1;
		if ( box.top < 0 ) box.top = 0;
		if ( box.bottom > (unsigned int)m_DesktopRes.y - 1 ) box.bottom = (unsigned int)m_DesktopRes.y - 1;

		m_pRenderer11->pImmPipeline->CopySubresourceRegion( m_RenderTarget[i], 0, 0, 0, 0, m_OffscreenTexture, 0, &box );
		m_pRenderer11->Present( m_pWindow[i]->GetHandle(), m_pWindow[i]->GetSwapChain() );
	}

}

void ExtendedWmlCamera::RotateLateral( float fAngle )
{
	Matrix3f rotate;
	rotate.FromAxisAngle( GetUp(), fAngle );

	Vector3f direction( m_ptTarget - GetLocation () );

	Vector3f newLeft( rotate * GetLeft() );
	newLeft.Normalize();

	SetTargetFrame( GetLocation(), newLeft, GetUp(), 
		GetLocation() + ( rotate * direction ) );
}

void FPSControls::setUpVector( const Vector3f& y )
{
    Matrix3f b = GeometryUtils::getRightHandedBasis( y );
    m_groundPlaneToWorld.setCol( 0, b.getCol( 1 ) );
    m_groundPlaneToWorld.setCol( 1, b.getCol( 2 ) );
    m_groundPlaneToWorld.setCol( 2, b.getCol( 0 ) );

    m_worldToGroundPlane = m_groundPlaneToWorld.inverse();

    // TODO: snap camera to face up when you change the up vector to something
    //   new rotate along current lookat direction?
    // TODO: reset camera
}

MovableTransProperty::MovableTransProperty (PropertyPage *parent, 
	const std::string &name, const std::string &tag,
	Transform *trans, Object *obj)
	:
Property(parent, name, tag, Property::PT_TRANSFORM, 0),
	mIsRSMatrix(0),
	mPropertyTranslate(0),
	mPropertyRotation(0),
	mPropertyScale(0),
	mPropertyIsUniformScale(0),
	mTrans(trans),
	mObject(obj)
{
	APoint position;
	APoint rotation;
	APoint scale(1.0f, 1.0f, 1.0f);

	bool isRSMatrix = mTrans->IsRSMatrix();
	if (isRSMatrix)
	{
		position = mTrans->GetTranslate();
		Matrix3f mat = mTrans->GetRotate();
		mat.ExtractEulerXYZ(rotation.X(), rotation.Y(), rotation.Z());
		scale = mTrans->GetScale();
		bool isUniformScale = mTrans->IsUniformScale();

		mProperty = parent->mPage->Append(new wxStringProperty(
			name, tag, wxT("<composed>")) );

		mPropertyTranslate = parent->mPage->AppendIn(mProperty, 
			new wxAPoint3Property("Translate", tag+"Translate",
			position));
		mPropertyRotation = parent->mPage->AppendIn(mProperty, 
			new wxAPoint3Property("Rotate", tag+"Rotate", rotation));
		mPropertyScale = parent->mPage->AppendIn(mProperty, 
			new wxAPoint3Property("Scale", tag+"Scale", scale));

		mPropertyIsUniformScale = parent->mPage->AppendIn(mProperty, 
			new wxBoolProperty("IsUniformScale", tag+"IsUniformScale", isUniformScale));
		mPropertyIsUniformScale->Enable(false);
	}
	else
	{
		mProperty = parent->mPage->Append(new wxStringProperty(
			name, tag, wxT("<composed>")) );

		mIsRSMatrix = parent->mPage->AppendIn(mProperty, 
			new wxBoolProperty("IsRSMatrix", tag+"IsRSMatrix", false));
		mIsRSMatrix->Enable(false);
	}
}