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

/* ==================== TO IMPLEMENT =======================
 * measurement(0) : x-position of marker in drone's xy-coordinate system (independant of roll, pitch)
 * measurement(1): y-position of marker in drone's xy-coordinate system (independant of roll, pitch)
 * measurement(2): yaw rotation of marker, in drone's xy-coordinate system (independant of roll, pitch)
 *
 * global_marker_pose(0): x-position or marker in world-coordinate system
 * global_marker_pose(1): y-position or marker in world-coordinate system
 * global_marker_pose(2): yaw-rotation or marker in world-coordinate system
 */
 void ExtendedKalmanFilter::correctionStep(const Eigen::Vector3f& measurement, const Eigen::Vector3f& global_marker_pose)
 {
	printf("TO IMPLEMENT! ekf: %f %f %f;    obs: %f %f %f\n",
			state[0],state[1],state[2],
			measurement[0], measurement[1], measurement[2]);

	float dx = global_marker_pose(0)-state(0); //dx = x_marker - x_drone_global
	float dy = global_marker_pose(1)-state(1);//dy = y_marker - y_drone_global
	float dYaw = global_marker_pose(2)-state(2);//dYaw = yaw_marker - yaw_drone_global

	// dh/dx:
	Eigen::Matrix3f H;

	H << -cos(state(2)), -sin(state(2)), -dx*sin(state(2))+dy*cos(state(2)),
		sin(state(2)), -cos(state(2)), -dx*cos(state(2))-dy*sin(state(2)),
		0, 0,  -1;

	//K
	Eigen::Matrix3f K, L;

	L = H*sigma*H.transpose()+R;
	K = sigma*H.transpose()*L.inverse();

	//update state
	//f = z-h(x)
	Eigen::Vector3f f;

	f << measurement(0)-cos(state(2))*dx-sin(state(2))*dy,
			measurement(1)+sin(state(2))*dx-cos(state(2))*dy,
			measurement(2)-dYaw;

	f(2) = atan2(sin(f(2)),cos(f(2)));  // normalize angle

	state = state + K*f;
	Eigen::Vector3f a;
	a = K*f;
	cout << "f: " << f << endl;
	cout << "K: " << K << endl;
	cout << "K*f" << a << endl;

	//update covariance matrix
	Eigen::Matrix3f I;
	I << 1,0,0,
			0,1,0,
			0,0,1;
	sigma = (I - K*H)*sigma;

}

void CCubicGrids::extractRTFromBuffer(const cuda::GpuMat& cvgmSumBuf_, Eigen::Matrix3f* peimRw_, Eigen::Vector3f* peivTw_) const{
	Mat cvmSumBuf;
	cvgmSumBuf_.download(cvmSumBuf);
	double* aHostTmp = (double*)cvmSumBuf.data;
	//declare A and b
	Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
	Eigen::Matrix<double, 6, 1> b;
	//retrieve A and b from cvmSumBuf
	short sShift = 0;
	for (int i = 0; i < 6; ++i){   // rows
		for (int j = i; j < 7; ++j) { // cols + b
			double value = aHostTmp[sShift++];
			if (j == 6)       // vector b
				b.data()[i] = value;
			else
				A.data()[j * 6 + i] = A.data()[i * 6 + j] = value;
		}//for each col
	}//for each row
	//checking nullspace
	double dDet = A.determinant();
	if (fabs(dDet) < 1e-15 || dDet != dDet){
		if (dDet != dDet)
			PRINTSTR("Failure -- dDet cannot be qnan. ");
		//reset ();
		return;
	}//if dDet is rational
	//float maxc = A.maxCoeff();

	Eigen::Matrix<float, 6, 1> result = A.llt().solve(b).cast<float>();
	//Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);

	float alpha = result(0);
	float beta = result(1);
	float gamma = result(2);

	Eigen::Matrix3f Rinc = (Eigen::Matrix3f)Eigen::AngleAxisf(gamma, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(beta, Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX());
	Eigen::Vector3f tinc = result.tail<3>();

	//compose
	//eivTwCur   = Rinc * eivTwCur + tinc;
	//eimrmRwCur = Rinc * eimrmRwCur;
	Eigen::Vector3f eivTinv = -peimRw_->transpose()* (*peivTw_);
	Eigen::Matrix3f eimRinv = peimRw_->transpose();
	eivTinv = Rinc * eivTinv + tinc;
	eimRinv = Rinc * eimRinv;
	*peivTw_ = -eimRinv.transpose() * eivTinv;
	*peimRw_ = eimRinv.transpose();
}

void createSymmetricProblems
(
	size_t nr_problems,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>* R
)
{
	// Random number generator
	std::mt19937_64 rng;
	std::uniform_real_distribution<float> d;
	
	for (int i = 0; i < (int)nr_problems; i++)
	{
		// Rest-state
		Eigen::Matrix3f M;
		M << d(rng), d(rng), d(rng),
		     d(rng), d(rng), d(rng),
		     d(rng), d(rng), d(rng);
		Eigen::Matrix3f MtM = M.transpose() * M;
		F.at<float>(i) = MtM;

		if (R)
		{
			Eigen::Matrix3f Rot;
			Vcl::Mathematics::PolarDecomposition(MtM, Rot, nullptr);
			R->template at<float>(i) = Rot;
		}
	}
}

void 
pcl::getCameraMatrixFromProjectionMatrix (
    const Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix, 
    Eigen::Matrix3f& camera_matrix)
{
  Eigen::Matrix3f KR = projection_matrix.topLeftCorner <3, 3> ();

  Eigen::Matrix3f KR_KRT = KR * KR.transpose ();
  
  Eigen::Matrix3f cam = KR_KRT / KR_KRT.coeff (8);

  memset (&(camera_matrix.coeffRef (0)), 0, sizeof (Eigen::Matrix3f::Scalar) * 9);
  camera_matrix.coeffRef (8) = 1.0;
  
  if (camera_matrix.Flags & Eigen::RowMajorBit)
  {
    camera_matrix.coeffRef (2) = cam.coeff (2);
    camera_matrix.coeffRef (5) = cam.coeff (5);
    camera_matrix.coeffRef (4) = static_cast<float> (std::sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5)));
    camera_matrix.coeffRef (1) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
    camera_matrix.coeffRef (0) = static_cast<float> (std::sqrt (cam.coeff (0) - camera_matrix.coeff (1) * camera_matrix.coeff (1) - cam.coeff (2) * cam.coeff (2)));
  }
  else
  {
    camera_matrix.coeffRef (6) = cam.coeff (2);
    camera_matrix.coeffRef (7) = cam.coeff (5);
    camera_matrix.coeffRef (4) = static_cast<float> (std::sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5)));
    camera_matrix.coeffRef (3) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
    camera_matrix.coeffRef (0) = static_cast<float> (std::sqrt (cam.coeff (0) - camera_matrix.coeff (3) * camera_matrix.coeff (3) - cam.coeff (2) * cam.coeff (2)));
  }
}

void ShapeMatch::setTarget(std::vector<double> &target_vec)
{
    target_mat = Eigen::Map<Eigen::Matrix3Xd>(&target_vec[0], 3, target_vec.size()/3);

    target_center = Eigen::Vector3d(target_mat.row(0).mean(), target_mat.row(0).mean(), target_mat.row(0).mean());
    target_mat.row(0) = (target_mat.row(0).array() - target_center(0)).matrix();
    target_mat.row(1) = (target_mat.row(1).array() - target_center(1)).matrix();
    target_mat.row(2) = (target_mat.row(2).array() - target_center(2)).matrix();

    // do icp alignment first
    //Eigen::MatrixXd C_n = Eigen::MatrixXd::Identity(target_mat.cols(), target_mat.cols()) - Eigen::MatrixXd::Ones(target_mat.cols(), target_mat.cols()) / target_mat.cols();
    Eigen::Matrix3d H = target_mat*template_mat.transpose();
    Eigen::Matrix3f H_float = H.cast<float>();
    Eigen::Matrix3f Ui;
    Eigen::Vector3f Wi;
    Eigen::Matrix3f Vi;
    wunderSVD3x3(H_float, Ui, Wi, Vi);
    Eigen::Matrix3d R = (Vi * Ui.transpose()).cast<double>();
    Eigen::Vector3d t = template_center - R*target_center;
    target_mat = R*target_mat + t*Eigen::MatrixXd::Ones(1, target_mat.cols());

    target_center = Eigen::Vector3d(target_mat.row(0).mean(), target_mat.row(0).mean(), target_mat.row(0).mean());
    target_mat.row(0) = (target_mat.row(0).array() - target_center(0)).matrix();
    target_mat.row(1) = (target_mat.row(1).array() - target_center(1)).matrix();
    target_mat.row(2) = (target_mat.row(2).array() - target_center(2)).matrix();

    target_vec = std::vector<double>(target_mat.data(), target_mat.data()+target_mat.cols()*target_mat.rows());
}

/** Get the inliers among all matches that comply with a given fundamental matrix.
 * @param std::vector<Match> vector with feature matches
 * @param Eigen::Matrix3f fundamental matrix
 * @return std::vector<int> vector with indices of the inliers */
std::vector<int> MonoOdometer5::getInliers(std::vector<Match> matches, Eigen::Matrix3f F)
{
    std::vector<int> inlierIndices;
    
    for(int i=0; i<matches.size(); i++)
    {
        cv::Point2f pPrev = matches[i].pPrev_;
        cv::Point2f pCurr = matches[i].pCurr_;
        Eigen::Vector3f pPrevHomog, pCurrHomog;
        pPrevHomog << pPrev.x, pPrev.y, 1.0;
        pCurrHomog << pCurr.x, pCurr.y, 1.0;

		// xCurr^T * F * xPrev 
        double x2tFx1 = pCurrHomog.transpose() * F * pPrevHomog;

		// F * xPrev
        Eigen::Vector3f Fx1 = F * pPrevHomog;
		
		// F^T * xCurr
        Eigen::Vector3f Ftx2 = F.transpose() * pCurrHomog;
        
		// compute Sampson distance (distance to epipolar line)
        double dSampson = (x2tFx1 * x2tFx1) / ((Fx1(0)*Fx1(0)) + (Fx1(1)*Fx1(1)) + (Ftx2(0)*Ftx2(0)) + (Ftx2(1)*Ftx2(1)));

        if(dSampson < param_odometerInlierThreshold_)
        {
            inlierIndices.push_back(i);
        }
    }
    return inlierIndices;
}

void PairwiseRegistrationDialog::showResults(const Eigen::Matrix4f &transformation, float rmsError, int corrNumber)
{
	std::stringstream temp;
	Eigen::Matrix4f T = transformation;
	temp.str("");
	temp << T;
	TTextEdit->setText(QString::fromStdString(temp.str()));
	Eigen::Matrix3f R = T.block(0,0,3,3);
	float c = (R * R.transpose()).diagonal().mean();
	c = qSqrt(c);
	cLineEdit->setText(QString::number(c));
	R /= c;
	temp.str("");
	temp << R;
	RTextEdit->setText(QString::fromStdString(temp.str()));
	Eigen::Vector3f t = T.block(0,3,3,1);
	temp.str("");
	temp << t;
	tLineEdit->setText(QString::fromStdString(temp.str()));
	Eigen::AngleAxisf angleAxis(R);
	angleLineEdit->setText( QString::number(angleAxis.angle()) );
	temp.str("");
	temp << angleAxis.axis();
	axisLineEdit->setText(QString::fromStdString(temp.str()));

	corrNumberLineEdit->setText(QString::number(corrNumber));
	errorLineEdit->setText(QString::number(rmsError));
}

/* compute Jacobian */
void mmf::OptSO3ApproxGD::ComputeJacobian(const SO3f& thetaPrev, const
    SO3f& theta, Eigen::Vector3f* J, float* f) {
  Eigen::Matrix3f R = theta.matrix();
  Eigen::Matrix3f Rprev = thetaPrev.matrix();

#ifndef NDEBUG
  cout<<"qKarch"<<endl<<qKarch_<<endl;
  cout<<"xSums_"<<endl<<xSums_<<endl;
  cout<<"Ns_"<<endl<<Ns_<<endl;
#endif

  if(J) for (uint32_t k=0; k<3; ++k)
      (*J)(k) = tauR_*(Rprev.transpose()*SO3f::G(k)*R).trace();
  if(f) *f = tauR_*(Rprev.transpose()*R).trace();

  for (uint32_t j=0; j<6; ++j) { 
    if (Ns_(j) == 0) continue;
    Eigen::Vector3f m = Eigen::Vector3f::Zero();
    m(j/2) = j%2==0?-1.:1.;
    float dot = max(-1.0f,min(1.0f,qKarch_.col(j).dot(R*m)));
    float eps = acos(dot);
    if (f) *f += Ns_(j)*eps*eps;
    if (J) {
      float factor = 0.;
      if(-0.99< dot && dot < 0.99)
        factor = -(2.*Ns_(j)*eps)/(sqrt(1.f-dot*dot));
      else if(dot >= 0.99) { 
        // taylor series around 0.99 according to Mathematica
        factor = -2.*Ns_(j)*(1.0033467240646519 - 0.33601724502488395
            *(-0.99 + dot) + 0.13506297338381046* (-0.99 + dot)*(-0.99 + dot));
      }else if (dot <= -0.99) {
        factor = -2.*Ns_(j)*(21.266813135156017 - 1108.2484926534892*(0.99
              + dot) + 83235.29739487475*(0.99 + dot)*(0.99 + dot));
      }
//      std::cout << "factor " << factor << std::endl;
      for (uint32_t k=0; k<3; ++k)
        (*J)(k) += factor*qKarch_.col(j).dot(SO3f::G(k)*R*m);
    }
  }
  if (f) *f /= -Ns_.sum();
  if (J) *J /= -Ns_.sum();
}

void ARAPTerm::updateRotation()
{
    Solver::DeformPetal& deform_petal = Solver::deform_petals_[petal_id_];
    Solver::PetalMatrix& origin_petal = deform_petal._origin_petal;
    Solver::PetalMatrix& petal_matrix = deform_petal._petal_matrix;
    Solver::RotList& rot_list = deform_petal._R_list;
    Solver::ScaleList& scale_list = deform_petal._S_list;
    Solver::AdjList& adj_list = deform_petal._adj_list;
    Solver::WeightMatrix& weight_matrix = deform_petal._weight_matrix;

    Eigen::Matrix3f Si;
    Eigen::MatrixXd Di;

    Eigen::Matrix3Xd Pi_Prime;
    Eigen::Matrix3Xd Pi;

    for (size_t i = 0, i_end = rot_list.size(); i < i_end; ++i) 
    {
        Di = Eigen::MatrixXd::Zero(adj_list[i].size(), adj_list[i].size());
        Pi_Prime.resize(3, adj_list[i].size());
        Pi.resize(3, adj_list[i].size());

        for (size_t j = 0, j_end = adj_list[i].size(); j < j_end; ++j) 
        {
            Di(j, j) = weight_matrix.coeffRef(i, adj_list[i][j]);
            Pi.col(j) = origin_petal.col(i) - origin_petal.col(adj_list[i][j]);
            Pi_Prime.col(j) = petal_matrix.col(i) - petal_matrix.col(adj_list[i][j]);
        }
        Si = Pi.cast<float>() * Di.cast<float>() * Pi_Prime.transpose().cast<float>();
        Eigen::Matrix3f Ui;
        Eigen::Vector3f Wi;
        Eigen::Matrix3f Vi;
        wunderSVD3x3(Si, Ui, Wi, Vi);
        rot_list[i] = Vi.cast<double>() * Ui.transpose().cast<double>();

        if (rot_list[i].determinant() < 0)
            std::cout << "determinant is negative!" << std::endl;

        double s = 0;
        for (size_t j = 0, j_end = adj_list[i].size(); j < j_end; ++j) 
        {
            s += Di(j, j) * Pi.col(j).squaredNorm();
        }

       // scale_list[i] = Wi.trace() / s;

       /* if (scale_list[i] < 0.95 )
            scale_list[i] = 0.95;
        else if (scale_list[i] > 1.05)
            scale_list[i] = 1.05;*/
    }

    /*if (petal_id_ == 0) std::cout << "vertex: " << 0 << "  " << scale_list[0] << std::endl;*/
}

	void CGLUtil::viewerGL()
	{
		glMatrixMode(GL_MODELVIEW);
		// load the matrix to set camera pose
		glLoadMatrixf(_eimModelViewGL.data());
		
		//rotation
		Eigen::Matrix3f eimRotation;
		if( btl::utility::BTL_GL == _eConvention ){
			eimRotation = Eigen::AngleAxisf(float(_dXAngle*M_PI/180.f), Eigen::Vector3f::UnitY())* Eigen::AngleAxisf(float(_dYAngle*M_PI/180.f), Eigen::Vector3f::UnitX());                         // 3. rotate horizontally
		}//mouse x-movement is the rotation around y-axis
		else if( btl::utility::BTL_CV == _eConvention )	{
			eimRotation = Eigen::AngleAxisf(float(_dXAngle*M_PI/180.f), -Eigen::Vector3f::UnitY())* Eigen::AngleAxisf(float(_dYAngle*M_PI/180.f), Eigen::Vector3f::UnitX());                         // 3. rotate horizontally
		}
		//translation
		/*_dZoom = _dZoom < 0.1? 0.1: _dZoom;
		_dZoom = _dZoom > 10? 10: _dZoom;*/

		//get direction N pointing from camera center to the object centroid
		Eigen::Affine3f M; M = _eimModelViewGL;
		Eigen::Vector3f T = M.translation();
		Eigen::Matrix3f R = M.linear();
		Eigen::Vector3f C = -R.transpose()*T;//camera centre
		Eigen::Vector3f N = _eivCentroid - C;//from camera centre to object centroid
		N = N/N.norm();//normalization

		Eigen::Affine3f _eimManipulate; _eimManipulate.setIdentity();
		_eimManipulate.translate( N*float(_dZoom) );//(N*(1-_dZoom));  //use camera movement toward object for zoom in/out effects
		_eimManipulate.translate(_eivCentroid);  // 5. translate back to the original camera pose
		//_eimManipulate.scale(s);				 // 4. zoom in/out, never use scale to simulate camera movement, it affects z-buffer capturing. use translation instead
		_eimManipulate.rotate(eimRotation);		 // 2. rotate vertically // 3. rotate horizontally
		_eimManipulate.translate(-_eivCentroid); // 1. translate the camera center to align with object centroid*/
		glMultMatrixf(_eimManipulate.data());

		/*	
		lTranslated( _aCentroid[0], _aCentroid[1], _aCentroid[2] ); // 5. translate back to the original camera pose
		_dZoom = _dZoom < 0.1? 0.1: _dZoom;
		_dZoom = _dZoom > 10? 10: _dZoom;
		glScaled( _dZoom, _dZoom, _dZoom );                      //  4. zoom in/out, 
		if( btl::utility::BTL_GL == _eConvention )
		glRotated ( _dXAngle, 0, 1 ,0 );                         // 3. rotate horizontally
		else if( btl::utility::BTL_CV == _eConvention )			//mouse x-movement is the rotation around y-axis
		glRotated ( _dXAngle, 0,-1 ,0 ); 
		glRotated ( _dYAngle, 1, 0 ,0 );                             // 2. rotate vertically
		glTranslated(-_aCentroid[0],-_aCentroid[1],-_aCentroid[2] ); // 1. translate the camera center to align with object centroid
		*/

		// light position in 3d
		glLightfv(GL_LIGHT0, GL_POSITION, _aLight);
	}

void CUDABuildLinearSystemRGBD::applyBL(CameraTrackingInput cameraTrackingInput, Eigen::Matrix3f& intrinsics, CameraTrackingParameters cameraTrackingParameters, float3 anglesOld, float3 translationOld, unsigned int imageWidth, unsigned int imageHeight, unsigned int level, Matrix6x7f& res, LinearSystemConfidence& conf) 
{
	unsigned int localWindowSize = 12;
	if(level != 0) localWindowSize = max(1, localWindowSize/(4*level));

	const unsigned int blockSize = 64;
	const unsigned int dimX = (unsigned int)ceil(((float)imageWidth*imageHeight)/(localWindowSize*blockSize));

	Eigen::Matrix3f intrinsicsRowMajor = intrinsics.transpose(); // Eigen is col major / cuda is row major
	computeNormalEquations(imageWidth, imageHeight, d_output, cameraTrackingInput, intrinsicsRowMajor.data(), cameraTrackingParameters, anglesOld, translationOld, localWindowSize, blockSize);

	cutilSafeCall(cudaMemcpy(h_output, d_output, sizeof(float)*30*dimX, cudaMemcpyDeviceToHost));

	// Copy to CPU
	res = reductionSystemCPU(h_output, dimX, conf);
}

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

/**
 * estimateRigidTransformationSVD
 */
void RigidTransformationRANSAC::estimateRigidTransformationSVD(
      const std::vector<Eigen::Vector3f > &srcPts,
      const std::vector<int> &srcIndices,
      const std::vector<Eigen::Vector3f > &tgtPts,
      const std::vector<int> &tgtIndices,
      Eigen::Matrix4f &transform)
{
  Eigen::Vector3f srcCentroid, tgtCentroid;

  // compute the centroids of source, target
  ComputeCentroid (srcPts, srcIndices, srcCentroid);
  ComputeCentroid (tgtPts, tgtIndices, tgtCentroid);

  // Subtract the centroids from source, target
  Eigen::MatrixXf srcPtsDemean;
  DemeanPoints(srcPts, srcIndices, srcCentroid, srcPtsDemean);

  Eigen::MatrixXf tgtPtsDemean;
  DemeanPoints(tgtPts, tgtIndices, tgtCentroid, tgtPtsDemean);

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

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

  // Return the transformation
  Eigen::Matrix3f R = v * u.transpose ();
  Eigen::Vector3f t = tgtCentroid - R * srcCentroid;

  // set rotation
  transform.block(0,0,3,3) = R;
  // set translation
  transform.block(0,3,3,1) = t;
  transform.block(3, 0, 1, 3).setZero();  
  transform(3,3) = 1.;
}

 // odometry:
 // x: distance traveled in local x-direction
 // y: distance traveled in local y-direction
 // phi: rotation update
 void ExtendedKalmanFilter::predictionStep(const Eigen::Vector3f& odometry)
 {

	state(0) = state(0) + cos(state(2))*odometry(0) - sin(state(2))*odometry(1);
	state(1) = state(1) + sin(state(2))*odometry(0) + cos(state(2))*odometry(1);
	state(2) = state(2) + odometry(2);

	state(2) = atan2(sin(state(2)),cos(state(2))); // normalize angle

	// dg/dx:
	Eigen::Matrix3f G;

	G << 1, 0, -sin(state(2))*odometry(0) - cos(state(2))*odometry(1),
		0, 1,  cos(state(2))*odometry(0) - sin(state(2))*odometry(1),
		0, 0,  1;

	sigma = G*sigma*G.transpose() + Q;
 }

void
drawBoundingBox(PointCloudT::Ptr cloud,
				boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer,
				int z)
{
	
	//Eigen::Vector4f centroid;
	pcl::compute3DCentroid(*cloud, centroid);
	
	//Eigen::Matrix3f covariance;
	computeCovarianceMatrixNormalized(*cloud, centroid, covariance);
	//Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance,
	//Eigen::ComputeEigenvectors);

	eigen_solver.compute(covariance,Eigen::ComputeEigenvectors);
	
//	eigen_solver = boost::shared_ptr<Eigen::SelfAdjointEigenSolver>
//		(covariance,Eigen::ComputeEigenvectors);

	eigDx = eigen_solver.eigenvectors();
    eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1));


	//Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity());
	p2w.block<3,3>(0,0) = eigDx.transpose();
	p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>());
    //pcl::PointCloud<PointT> cPoints;
    pcl::transformPointCloud(*cloud, cPoints, p2w);


	//PointT min_pt, max_pt;
    pcl::getMinMax3D(cPoints, min_pt, max_pt);
    const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());

	const Eigen::Quaternionf qfinal(eigDx);
    const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>();
	
	//viewer->addPointCloud(cloud);
	viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z,boost::lexical_cast<std::string>((z+1)*200));

}