C++ Matrix3f::col方法代码示例

Matrix3f Segment::jacobianMatrix() {
    Matrix3f result;
    result.col(0) = jacobianColumn(getX());
    result.col(1) = jacobianColumn(getY());
    result.col(2) = jacobianColumn(getZ());
    return result;
}

/**************************************************************************************************
 *  Procecure                                                                                     *
 *                                                                                                *
 *  Description: swapColumns                                                                      *
 *  Class      : ApproachMovementSpace                                                            *
 **************************************************************************************************/
Matrix3f ApproachMovementSpace::swapColumns(Matrix3f matrix, uint column1, uint column2)
{
	Vector3f aux        = matrix.col(column1);

	matrix.col(column1) = matrix.col(column2);
	matrix.col(column2) = aux;

	return matrix;
}

SceneObject& SceneObject::setRotation(const Vector3f& x, const Vector3f& y, const Vector3f& z) {
    Matrix3f M;
    M.col(0) = x;
    M.col(1) = y;
    M.col(2) = z;
    rotation = Quaternionf(M);
    rotation.normalize();
    return *this;
}

void FakeKinect::setWorldFromCam(const Eigen::Affine3f &worldFromCam) {
  Vector3f eye, center, up;
  Matrix3f rot;
  rot = worldFromCam.linear();
  eye = worldFromCam.translation();
  center = rot.col(2);
  up = -rot.col(1);
  m_cam->setViewMatrixAsLookAt(toOSGVector(eye), toOSGVector(center+eye), toOSGVector(up));
}

/* 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 Camera::setDirection(const Vector3f& newDirection)
{
    // TODO implement it computing the rotation between newDirection and current dir ?
    Vector3f up = this->up();
    
    Matrix3f camAxes;

    camAxes.col(2) = (-newDirection).normalized();
    camAxes.col(0) = up.cross( camAxes.col(2) ).normalized();
    camAxes.col(1) = camAxes.col(2).cross( camAxes.col(0) ).normalized();
    setOrientation(Quaternionf(camAxes));
    
    mViewIsUptodate = false;
}

void Camera::lookAt(const Vector3f &center, const Vector3f &eye, Vector3f &up) {
    Vector3f z = eye - center;
    z.normalize();
    up.normalize();
    Vector3f x = up.cross(z);
    x.normalize();
    Vector3f y = z.cross(x);
    Matrix3f M;
    M.col(0) = x;
    M.col(1) = y;
    M.col(2) = z;
    std::cout << M << std::endl;
    rotation = Quaternionf(M);
    position = eye;
}

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;
}

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;
//  }
};

float mmf::OptSO3ApproxGD::conjugateGradientPreparation_impl(Matrix3f& R, uint32_t& N)
{
//  std::cout << "R before" << std::endl;
//  std::cout << R << std::endl;
//  Timer t0;
  N =0;
  float res0 = this->computeAssignment(R,N)/float(N);
  if(this->t_ == 0){
   // init karcher means with columns of the rotation matrix (takes longer!)
    qKarch_ << R.col(0),-R.col(0),R.col(1),-R.col(1),R.col(2),-R.col(2);
//    // init karcher means with rotated version of previous karcher means
//    qKarch_ =  (this->Rprev_*R.transpose())*qKarch_; 
  }
  qKarch_ = karcherMeans(qKarch_, 5.e-5, 10);
//  t0.toctic("----- karcher mean");
  // compute a rotation matrix from the karcher means (not necessary)
//  Matrix3f Rkarch;
//  Rkarch.col(0) =  qKarch_.col(0);
//  Rkarch.col(1) =  qKarch_.col(2) - qKarch_.col(2).dot(qKarch_.col(0))
//    *qKarch_.col(0);
//  Rkarch.col(1) /= Rkarch.col(1).norm();
//  Rkarch.col(2) = Rkarch.col(0).cross(Rkarch.col(1));
#ifndef NDEBUG
  cout<<"R: "<<endl<<R<<endl;
//  cout<<"Rkarch: "<<endl<<Rkarch<<endl<<"det(Rkarch)="<<Rkarch.determinant()<<endl;
  std::cout << "qKarch_" << std::endl;
  std::cout << qKarch_ << std::endl;
#endif
//  t0.tic();
  computeSuffcientStatistics();
//  t0.toctic("----- sufficient statistics");
  return res0; // cost fct value
}

void MiniGL::setViewport (float pfovy, float pznear, float pzfar, const Vector3f &peyepoint, const Vector3f &plookat)
{
	fovy = pfovy;
	znear = pznear;
	zfar = pzfar;

	glLoadIdentity ();
	gluLookAt (peyepoint [0], peyepoint [1], peyepoint [2], plookat[0], plookat[1], plookat[2], 0, 1, 0);

	Matrix4f transformation;
	float *lookAtMatrix = transformation.data();
	glGetFloatv (GL_MODELVIEW_MATRIX, &lookAtMatrix[0]);
	
	Matrix3f rot;
	Vector3f scale;

	transformation.transposeInPlace();

	rot.row(0) = Vector3f (transformation(0,0), transformation(0,1), transformation(0,2));
	rot.row(1) = Vector3f (transformation(1,0), transformation(1,1), transformation(1,2));
	rot.row(2) = Vector3f (transformation(2,0), transformation(2,1), transformation(2,2));
	scale[0] = rot.col(0).norm();
	scale[1] = rot.col(1).norm();
	scale[2] = rot.col(2).norm();
	m_translation = Vector3f (transformation(3,0), transformation(3,1), transformation(3,2));

	rot.col(0) = 1.0f/scale[0] * rot.col(0);
	rot.col(1) = 1.0f/scale[1] * rot.col(1);
	rot.col(2) = 1.0f/scale[2] * rot.col(2);

	m_zoom = scale[0];
	m_rotation = Quaternionf(rot);

	glLoadIdentity ();
}

    void initTable(ColorCloudPtr cloud) {
        MatrixXf corners = getTableCornersRansac(cloud);

        Vector3f xax = corners.row(1) - corners.row(0);
        xax.normalize();
        Vector3f yax = corners.row(3) - corners.row(0);
        yax.normalize();
        Vector3f zax = xax.cross(yax);

        float zsgn = (zax(2) > 0) ? 1 : -1;
        xax *= - zsgn;
        zax *= - zsgn; // so z axis points up

        m_axes.col(0) = xax;
        m_axes.col(1) = yax;
        m_axes.col(2) = zax;

        MatrixXf rotCorners = corners * m_axes;

        m_mins = rotCorners.colwise().minCoeff();
        m_maxes = rotCorners.colwise().maxCoeff();
        m_mins(2) = rotCorners(0,2) + LocalConfig::zClipLow;
        m_maxes(2) = rotCorners(0,2) + LocalConfig::zClipHigh;



        m_transform.setBasis(btMatrix3x3(
                                 xax(0),yax(0),zax(0),
                                 xax(1),yax(1),zax(1),
                                 xax(2),yax(2),zax(2)));
        m_transform.setOrigin(btVector3(corners(0,0), corners(0,1), corners(0,2)));

        m_poly.points = toROSPoints32(toBulletVectors(corners));



        m_inited = true;

    }

/* evaluate cost function for a given assignment of npormals to axes */
float mmf::OptSO3ApproxGD::evalCostFunction(Matrix3f& R)
{
  float c = 0.0f;
  for (uint32_t j=0; j<6; ++j)
  { 
    const float dot = max(-1.0f,min(1.0f,(qKarch_.col(j).transpose() * R.col(j/2))(0)));
    if(j%2 ==0){
      c += Ns_(j) * acos(dot)* acos(dot);
    }else{
      c += Ns_(j) * acos(-dot)* acos(-dot);
    }
  }
  return c;
}

int main()
{
    Matrix3f m;
    m << 1, 2, 3,
         4, 5, 6,
         7, 8, 9;
    std::cout << m << std::endl;

    RowVectorXd vec1(3);
    vec1 << 1, 2, 3;
    std::cout << "vec1 = " << vec1 << std::endl;

    RowVectorXd vec2(4);
    vec2 << 1, 4, 9, 16;;
    std::cout << "vec2 = " << vec2 << std::endl;

    RowVectorXd joined(7);
    joined << vec1, vec2;
    std::cout << "joined = " << joined << std::endl;


    MatrixXf matA(2, 2);
    matA << 1, 2, 3, 4;
    MatrixXf matB(4, 4);
    matB << matA, matA/10, matA/10, matA;
    std::cout << matB << std::endl;

    {
    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 << std::endl;
    }

    return 0;
}

/**************************************************************************************************
 *  Procecure                                                                                     *
 *                                                                                                *
 *  Description: gramSchmidtNormalization                                                         *
 *  Class      : ApproachMovementSpace                                                            *
 **************************************************************************************************/
Matrix3f ApproachMovementSpace::gramSchmidtNormalization3f(Matrix3f matrix)
{
	// cout << endl << "Non-Normalized Matrix: " << endl << matrix << endl;

	uint matrix_size = matrix.diagonalSize();

	for(uint column = 0; column < matrix_size; column += 1)
	{
		// Step 1: Normalize vector
		matrix.col(column) = normalizeVector3f( matrix.col(column) );

		// Step 2: Eliminate the projection of this vector
		for(uint column_proj = column + 1; column_proj < matrix_size; column_proj += 1)
		{
			Vector3f vj = matrix.col(column_proj);

			matrix.col(column_proj) -= vj.dot( matrix.col(column) ) * matrix.col(column);
		}
	}

	// cout << endl << "Normalized Matrix: " << endl << matrix << endl;

	return matrix;
}