Beispiel #1
0
/* ==================== 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();
}
Beispiel #3
0
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));
}
Beispiel #8
0
template <typename PointSource, typename PointTarget> void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::getTransformationFromCorrelation (
    const Eigen::MatrixXf &cloud_src_demean,
    const Eigen::Vector4f &centroid_src,
    const Eigen::MatrixXf &cloud_tgt_demean,
    const Eigen::Vector4f &centroid_tgt,
    Eigen::Matrix4f &transformation_matrix)
{
  transformation_matrix.setIdentity ();

  // 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
  transformation_matrix.topLeftCorner<3, 3> () = R;
  Eigen::Vector3f Rc = R * centroid_src.head<3> ();
  transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
}
/* 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);
}
Beispiel #13
0
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.;
}
Beispiel #15
0
 // 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));

}
  void PinholePointProjector::unProject(PointVector &points, 
					Gaussian3fVector &gaussians,
					IntImage &indexImage,
					const DepthImage &depthImage) const {
    assert(depthImage.rows > 0 && depthImage.cols > 0 && "PinholePointProjector: Depth image has zero dimensions");
    points.resize(depthImage.rows * depthImage.cols);
    gaussians.resize(depthImage.rows * depthImage.cols);
    indexImage.create(depthImage.rows, depthImage.cols);
    int count = 0;
    Point *point = &points[0];
    Gaussian3f *gaussian = &gaussians[0];
    float fB = _baseline * _cameraMatrix(0, 0);
    Eigen::Matrix3f J;
    for(int r = 0; r < depthImage.rows; r++) {
      const float *f = &depthImage(r, 0);
      int *i = &indexImage(r, 0);
      for(int c = 0; c < depthImage.cols; c++, f++, i++) {      
	if(!_unProject(*point, c, r, *f)) {
	  *i = -1;
	  continue;
	}
	float z = *f;
	float zVariation = (_alpha * z * z) / (fB + z * _alpha);
	J <<       
	  z, 0, (float)r,
	  0, z, (float)c,
	  0, 0, 1;
	J = _iK * J;
	Diagonal3f imageCovariance(3.0f, 3.0f, zVariation);
	Eigen::Matrix3f cov = J * imageCovariance * J.transpose();
	*gaussian = Gaussian3f(point->head<3>(), cov);
	gaussian++;
	point++;
	*i = count;
	count++;
      }
    }

    points.resize(count);
    gaussians.resize(count);
  }
Beispiel #18
0
bool Utils::
factorViewMatrix(const Eigen::Projective3f& iMatrix,
                 Eigen::Matrix3f& oCalib, Eigen::Isometry3f& oPose,
                 bool& oIsOrthographic) {
  oIsOrthographic = isOrthographic(iMatrix.matrix());

  // get appropriate rows
  std::vector<int> rows = {0,1,2};
  if (!oIsOrthographic) rows[2] = 3;

  // get A matrix (upper left 3x3) and t vector
  Eigen::Matrix3f A;
  Eigen::Vector3f t;
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      A(i,j) = iMatrix(rows[i],j);
    }
    t[i] = iMatrix(rows[i],3);
  }

  // determine translation vector
  oPose.setIdentity();
  oPose.translation() = -(A.inverse()*t);

  // determine calibration matrix
  Eigen::Matrix3f AAtrans = A*A.transpose();
  AAtrans.col(0).swap(AAtrans.col(2));
  AAtrans.row(0).swap(AAtrans.row(2));
  Eigen::LLT<Eigen::Matrix3f, Eigen::Upper> llt(AAtrans);
  oCalib = llt.matrixU();
  oCalib.col(0).swap(oCalib.col(2));
  oCalib.row(0).swap(oCalib.row(2));
  oCalib.transposeInPlace();

  // compute rotation matrix
  oPose.linear() = (oCalib.inverse()*A).transpose();

  return true;
}
Beispiel #19
0
///
/// @par Detailed description 
/// ... 
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
Eigen::Matrix3f
sasmath::Math::
find_u(sasmol::SasMol &mol, int &frame)
{

    Eigen::Matrix3f r ;

    float rxx = (mol._x().col(frame)*_x().col(frame)).sum() ; 
    float rxy = (mol._x().col(frame)*_y().col(frame)).sum() ;
    float rxz = (mol._x().col(frame)*_z().col(frame)).sum() ;
    
    float ryx = (mol._y().col(frame)*_x().col(frame)).sum() ;
    float ryy = (mol._y().col(frame)*_y().col(frame)).sum() ;
    float ryz = (mol._y().col(frame)*_z().col(frame)).sum() ;

    float rzx = (mol._z().col(frame)*_x().col(frame)).sum() ;
    float rzy = (mol._z().col(frame)*_y().col(frame)).sum() ;
    float rzz = (mol._z().col(frame)*_z().col(frame)).sum() ;

    r << rxx,rxy,rxz,
         ryx,ryy,ryz,
         rzx,rzy,rzz;

    Eigen::Matrix3f rtr = r.transpose() * r ;
    
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(rtr);

    if (eigensolver.info() != Eigen::Success)
    {
          std::cout << "find_u calculation failed" << std::endl ;
    }

    Eigen::Matrix<float,3,1> uk ;  // eigenvalues
    Eigen::Matrix3f ak ;          // eigenvectors

    uk = eigensolver.eigenvalues() ; 
    ak = eigensolver.eigenvectors() ;

    Eigen::Matrix3f akt = ak.transpose() ;
    Eigen::Matrix3f new_ak  ;

    new_ak.row(0) = akt.row(2) ; //sort eigenvectors
    new_ak.row(1) = akt.row(1) ;
    new_ak.row(2) = akt.row(0) ;

    // python ak0 --> ak[2] ; ak1 --> ak[1] ; ak2 --> ak[0]

    //Eigen::Matrix<float,3,1> ak2 = akt.row(2).cross(akt.row(1)) ;

    Eigen::MatrixXf rak0 = (r * (akt.row(2).transpose())) ;
    Eigen::MatrixXf rak1 = (r * (akt.row(1).transpose())) ;

    Eigen::Matrix<float,3,1> urak0 ; 
    if(uk[2] == 0.0)
    { 
        urak0 = 1E15 * rak0 ;
    } 
    else
    {    
        urak0 = (1.0/sqrt(fabs(uk[2])))*rak0 ;
    }
    
    Eigen::Matrix<float,3,1> urak1 ; 
    if(uk[1] == 0.0)
    { 
        urak1 = 1E15 * rak1 ;
    } 
    else
    {    
        urak1 = (1.0/sqrt(fabs(uk[1])))*rak1 ;
    }

    Eigen::Matrix<float,3,1> urak2 = urak0.col(0).cross(urak1.col(0)) ;
    Eigen::Matrix3f bk ;

    bk << urak0,urak1,urak2;

    Eigen::Matrix3f u ;

    u =  (bk * new_ak).transpose() ;

    return u ;
/*
u = array([[-0.94513198,  0.31068658,  0.100992  ],
       [-0.3006246 , -0.70612572, -0.64110165],
       [-0.12786863, -0.63628635,  0.76078203]])
check:

u = 
-0.945133  0.310686  0.100992
-0.300624 -0.706126 -0.641102
-0.127869 -0.636287  0.760782 
*/
}
Beispiel #20
0
Eigen::VectorXf solve_ik(Eigen::Matrix3f goalR, Eigen::Vector3f goalT,int sol_index)
{
	
	cout << "[SOLVER] Solution " << sol_index << std::endl; 
	
  Eigen::VectorXf result(5);
  //~ index:
  //~ 0   direct space, low elbow
  //~ 1   direct space, high elbow
  //~ 2   inverse space, high elbow
  //~ 3   inverse space, low elbow
  
  Eigen::Matrix<float,5,4> DH;
  DH.row(0) << 0.0,   0.019,  0.0330,   M_PI/2;
  DH.row(1) << 0.0,   0.0,    0.1550,   0.0;
  DH.row(2) << 0.0,   0.0,    0.1350,   0.0;
  DH.row(3) << 0.0,   0.0,    0.0,      M_PI/2;
  DH.row(4) << 0.0,   0.2175, 0.0,      0.0;
  
  //~ from homing to DH structure
  //~ offset =  [deg2rad(169) deg2rad(65+90) -deg2rad(146) deg2rad(102-90) deg2rad(167)];
  
  Eigen::VectorXf offset(5);
  offset(0) = 169*DEG2RAD;
  offset(1) = 155*DEG2RAD;
  offset(2) = -146*DEG2RAD;
  offset(3) = 192*DEG2RAD;
  offset(4) = 167*DEG2RAD;
  
  float L2 = 0.155;
  float L3 = 0.135;
  float L4 = 0.2175;
  float beta = atan2(goalR(2,2), sqrt( pow(goalR(0,2),2) + pow(goalR(1,2),2) ) );
  
  float q1, q2, q3, q4, q5, Z1, Z2, X1, X2;
  float sin_theta3, cos_theta3, theta3, theta2;
  
  
  if (sol_index == 0 || sol_index == 1)
  {
    //~ direct arm space
    q1 = atan2(goalT(1),goalT(0));
    Z1 = goalT(2) - DH(0,1);
    Z2 = Z1 - L4*sin(beta);
    X1 = sqrt( pow(goalT(0),2) + pow(goalT(1),2)  ) - DH(0,2);
    X2 = X1 - L4*cos(beta);
  }
  else
  {
    //~ inverse arm space
    q1 = atan2(goalT(1),goalT(0)) - M_PI;
    Z1 = goalT(2) - DH(0,1);
    Z2 = Z1 - L4*sin(beta);
    X1 = sqrt( pow(goalT(0),2) + pow(goalT(1),2)  ) + DH(0,2);
    X2 = X1 - L4*cos(beta);
  }
  
  cos_theta3 = (-pow(Z2,2) -pow(X2,2) + pow(L2,2) + pow(L3,2)) / (2*L2*L3);
  
  if (cos_theta3 > -1 && cos_theta3 < 1)
  {
    // joint 3
    if (sol_index == 0 || sol_index == 2)
    {   
      sin_theta3 = sqrt(1-pow(cos_theta3,2));
      theta3 = atan2(sin_theta3,cos_theta3); 
      q3 = M_PI - theta3;
    }
    else
    {
      sin_theta3 = -sqrt(1-pow(cos_theta3,2));
      theta3 = atan2(sin_theta3,cos_theta3);
      q3 = - M_PI - theta3;
    }
    
    // joint 2
    float k1 = L2 + L3*cos(q3);
    float k2 = L3*sin(q3);
    
    if (sol_index == 0 || sol_index == 1)
    {    
      theta2 = atan2(Z2,X2) - atan2(k2,k1);
      q2 = theta2;
      // joint 4
      q4 = (M_PI/2) - (q2 + q3 - beta);
    }
    else
    {
      theta2 = atan2(Z2,X2) + atan2(k2,k1);
      q2 = M_PI - theta2;
      // joint 4
      q4 = ( M_PI - (q2 + q3) - beta) + M_PI/2;
    }
    
    
    Eigen::MatrixXf T(4,4);
    T = Eigen::MatrixXf::Identity(4,4);
    Eigen::Vector4f joints(q1,q2,q3,q4);
    float j, d, a, alpha;
    for (int k=0; k<4; ++k)
    {
      j = joints(k);
      d = DH(k,1);
      a = DH(k,2);
      alpha = DH(k,3);
      
      // component transform matrix
      Eigen::Matrix<float,4,4> CTM;
      CTM.row(0) << cos(j), -sin(j)*cos(alpha),   sin(j)*sin(alpha),    a*cos(j);
      CTM.row(1) << sin(j), cos(j)*cos(alpha),    -cos(j)*sin(alpha),   a*sin(j);    
      CTM.row(2) << 0.0,        sin(alpha),               cos(alpha),               d;
      CTM.row(3) << 0.0,        0.0,                      0.0,                      1;
      
      T = T * CTM;
    }
    Eigen::Matrix3f tool = T.block(0,0,3,3);
    Eigen::Matrix3f roll =  tool.transpose() * goalR;
    q5 = atan2(roll(1,0),roll(0,0));
    T = Eigen::MatrixXf::Identity(4,4);

    //~ The setpoint angle for joint arm_joint_1 is out of range. The valid range is between 0.0100692 rad and 5.84014 rad 
    //~ The setpoint angle for joint arm_joint_2 is out of range. The valid range is between 0.0100692 rad and 2.61799 rad 
    //~ The setpoint angle for joint arm_joint_3 is out of range. The valid range is between -5.02655 rad and -0.015708 rad 
    //~ The setpoint angle for joint arm_joint_4 is out of range. The valid range is between 0.0221239 rad and 3.4292 rad 
    //~ The setpoint angle for joint arm_joint_5 is out of range. The valid range is between 0.110619 rad and 5.64159 rad 
  
    result(0) = offset(0)-q1;
    result(1) = offset(1)-q2;
    result(2) = offset(2)-q3;
    result(3) = offset(3)-q4;
    result(4) = offset(4)-q5;
    
    if (result(4) < 0.110619 )
    {
			result(4) = result(4) + M_PI;
		}
		else if ( result(4) > 5.64159)
		{
			result(4) = result(4) - M_PI;
		}
  }
  else
  {
    // position not reachable: arm will stand still (we will give impossible values for the default controller)
    result = result.setConstant(-10);
  
  }
  
  Eigen::Matrix<float,5,2> range;
  range.row(0) << 0.0100692+0.0017,  5.84014-0.0017;
  range.row(1) << 0.0100692+0.0017,  2.61799-0.0017;
  range.row(2) << -5.02655+0.0017,   -0.015708-0.0017;
  range.row(3) << 0.0221239+0.0017,  3.4292-0.0017; 
  range.row(4) << 0.110619+0.0017,   5.64159-0.0017; 
  
  for(int k=0; k<5; ++k)
  {
    if( result(k) == -10)
    {
      cout << "[SOLVER] Solution " << sol_index << " discarded: desired goal not reachable by the robot" << std::endl;
      result = result.setZero();
      k = 5;
    }
    else if( range(k,0) > result(k) || range(k,1) < result(k) )
    {
      cout << "[SOLVER] Solution " << sol_index << " discarded: joint " << k+1 << " outside accetable ranges. Value: " << result(k) << "; Range: [" << range(k,0) << " , " << range(k,1) << "]" << std::endl;
      result = result.setZero();
      k = 5;
    }
    else
    {
    }
  }
  cout << "----------" << std::endl;
	return result;  
}
  void TwoDepthImageAlignerNode::processNode(MapNode* node_){
   cerr << "START ITERATION" << endl;
   std::vector<Serializable*> crearedObjects;
    SensingFrameNode* sensingFrame = dynamic_cast<SensingFrameNode*>(node_);
    if (! sensingFrame)
      return;
    
    PinholeImageData* image = dynamic_cast<PinholeImageData*>(sensingFrame->sensorData(_topic));
    if (! image)
      return;
    cerr << "got image"  << endl;
    
    Eigen::Isometry3d _sensorOffset = _config->sensorOffset(image->baseSensor());
    
    // cerr << "sensorOffset: " << endl;
    // cerr << _sensorOffset.matrix() << endl;

    Eigen::Isometry3f sensorOffset;
    convertScalar(sensorOffset,_sensorOffset);
    sensorOffset.matrix().row(3) << 0,0,0,1;

    Eigen::Matrix3d _cameraMatrix = image->cameraMatrix();
    
    ImageBLOB* blob = image->imageBlob().get();
    
    DepthImage depthImage;
    depthImage.fromCvMat(blob->cvImage());
    int r=depthImage.rows();
    int c=depthImage.cols();
    
    DepthImage scaledImage;
    Eigen::Matrix3f cameraMatrix;
    convertScalar(cameraMatrix,_cameraMatrix);
    
    //computeScaledParameters(r,c,cameraMatrix,_scale);
    PinholePointProjector* projector=dynamic_cast<PinholePointProjector*>(_converter->projector());
    cameraMatrix(2,2)=1;
    projector->setCameraMatrix(cameraMatrix);
    projector->setImageSize(depthImage.rows(), depthImage.cols());
    projector->scale(1.0/_scale);

    DepthImage::scale(scaledImage,depthImage,_scale);
    pwn::Frame* frame = new pwn::Frame;
    _converter->compute(*frame,scaledImage, sensorOffset);
  
    MapNodeBinaryRelation* odom=0;

    std::vector<MapNode*> oneNode(1);
    oneNode[0]=sensingFrame;
    MapNodeUnaryRelation* imu = extractRelation<MapNodeUnaryRelation>(oneNode);
    
    if (_previousFrame){
      _aligner->setReferenceSensorOffset(_aligner->currentSensorOffset());
      _aligner->setCurrentSensorOffset(sensorOffset);
      _aligner->setReferenceFrame(_previousFrame);
      _aligner->setCurrentFrame(frame);
      
      PinholePointProjector* projector=(PinholePointProjector*)(_aligner->projector());
      projector->setCameraMatrix(cameraMatrix);
      projector->setImageSize(depthImage.rows(), depthImage.cols());
      projector->scale(1.0/_scale);
      _aligner->correspondenceFinder()->setImageSize(projector->imageRows(), projector->imageCols());

      /*
	cerr << "correspondenceFinder: "  << r << " " << c << endl; 
	cerr << "sensorOffset" << endl;
	cerr <<_aligner->currentSensorOffset().matrix() << endl;
	cerr <<_aligner->referenceSensorOffset().matrix() << endl;
	cerr << "cameraMatrix" << endl;
	cerr << projector->cameraMatrix() << endl;
      */

      std::vector<MapNode*> twoNodes(2);
      twoNodes[0]=_previousSensingFrameNode;
      twoNodes[1]=sensingFrame;
      odom = extractRelation<MapNodeBinaryRelation>(twoNodes);
      cerr << "odom:" << odom << " imu:" << imu << endl;

      Eigen::Isometry3f guess= Eigen::Isometry3f::Identity();
      _aligner->clearPriors();
      if (odom){
      	Eigen::Isometry3f mean;
      	Eigen::Matrix<float,6,6> info;
      	convertScalar(mean,odom->transform());
	mean.matrix().row(3) << 0,0,0,1;
	convertScalar(info,odom->informationMatrix());
	//cerr << "odom: " << t2v(mean).transpose() << endl;
	_aligner->addRelativePrior(mean,info);
 	//guess = mean;
      } 

      if (imu){
      	Eigen::Isometry3f mean;
      	Eigen::Matrix<float,6,6> info;
      	convertScalar(mean,imu->transform());
      	convertScalar(info,imu->informationMatrix());
	mean.matrix().row(3) << 0,0,0,1;
	//cerr << "imu: " << t2v(mean).transpose() << endl;
	_aligner->addAbsolutePrior(_globalT,mean,info);
      }
      _aligner->setInitialGuess(guess);
      cerr << "Frames: " << _previousFrame << " " << frame << endl;

      
      // projector->setCameraMatrix(cameraMatrix);
      // projector->setTransform(Eigen::Isometry3f::Identity());
      // Eigen::MatrixXi debugIndices(r,c);
      // DepthImage debugImage(r,c);
      // projector->project(debugIndices, debugImage, frame->points());

      _aligner->align();
      
      // sprintf(buf, "img-dbg-%05d.pgm",j);
      // debugImage.save(buf);
      //sprintf(buf, "img-ref-%05d.pgm",j);
      //_aligner->correspondenceFinder()->referenceDepthImage().save(buf);
      //sprintf(buf, "img-cur-%05d.pgm",j);
      //_aligner->correspondenceFinder()->currentDepthImage().save(buf);

      cerr << "inliers: " << _aligner->inliers() << endl;
      cerr << "chi2: " << _aligner->error() << endl;
      cerr << "chi2/inliers: " << _aligner->error()/_aligner->inliers() << endl;
      cerr << "initialGuess: " << t2v(guess).transpose() << endl;
      cerr << "transform   : " << t2v(_aligner->T()).transpose() << endl;
      if (_aligner->inliers()>-1){
 	_globalT = _globalT*_aligner->T();
	cerr << "TRANSFORM FOUND" <<  endl;
      } else {
	cerr << "FAILURE" <<  endl;
	_globalT = _globalT*guess;
      }
      if (! (_counter%50) ) {
	Eigen::Matrix3f R = _globalT.linear();
	Eigen::Matrix3f E = R.transpose() * R;
	E.diagonal().array() -= 1;
	_globalT.linear() -= 0.5 * R * E;
      }
      _globalT.matrix().row(3) << 0,0,0,1;
      cerr << "globalTransform   : " << t2v(_globalT).transpose() << endl;

      // char buf[1024];
      // sprintf(buf, "frame-%05d.pwn",_counter);
      // frame->save(buf, 1, true, _globalT);


      cerr << "creating alias" << endl;
      // create an alias for the previous node
      MapNodeAlias* newRoot = new MapNodeAlias(_previousSensingFrameNode,_previousSensingFrameNode->manager());
      _previousSensingFrameNode->manager()->addNode(newRoot);
      
      cerr << "creating alias relation" << endl;
      MapNodeAliasRelation* aliasRel = new MapNodeAliasRelation(newRoot,_previousSensingFrameNode->manager());
      aliasRel->nodes()[0] = newRoot;
      aliasRel->nodes()[1] = _previousSensingFrameNode;
      _previousSensingFrameNode->manager()->addRelation(aliasRel);
      
      cerr << "reparenting old elements" << endl;
      // assign all the used relations to the alias node
      if (imu){
	imu->setOwner(newRoot);
	imu->manager()->addRelation(imu);
      }

      if (odom){
	odom->setOwner(newRoot);
	odom->manager()->addRelation(imu);
      }
      
      cerr << "adding result" << endl;
      
      Relation* newRel = new Relation(_aligner, _converter, 
	       _previousSensingFrameNode, sensingFrame,
	       _topic, _aligner->inliers(), _aligner->error(), _manager);
      newRel->setOwner(newRoot);
      newRel->nodes()[0] = newRoot;
      newRel->nodes()[1] = _previousSensingFrameNode;
      Eigen::Isometry3d iso;
      convertScalar(iso,_aligner->T());
      newRel->setTransform(iso);
      newRel->setInformationMatrix(Eigen::Matrix<double, 6,6>::Identity());
      newRel->manager()->addRelation(newRel);
      

      *os << _globalT.translation().transpose() << endl;

      // create a new alias node for the prior element
      // bind the alias with the prior node

      // add to the alias the relations that have been used to
      // determine the transformation

      // add the alias to the map
      // add the new transformation to the map
      
    } else {
      _aligner->setCurrentSensorOffset(sensorOffset);
      _globalT = Eigen::Isometry3f::Identity();
      if (imu){
      	Eigen::Isometry3f mean;
      	convertScalar(mean,imu->transform());
	_globalT = mean;
      }
    }
    
    cerr << "deleting previous frame" << endl;
    if (_previousFrame)
      delete _previousFrame;
    
    cerr << "deleting blob frame" << endl;
    delete blob;

    cerr << "bookkeeping update" << endl;
    _previousSensingFrameNode = sensingFrame;
    _previousFrame = frame;
    _counter++;
    
    cerr << "END ITERATION" << endl;
  }
Beispiel #22
0
void
LocalRecognitionPipeline<PointT>::correspondenceGrouping ()
{
    if(cg_algorithm_->getRequiresNormals() && (!scene_normals_ || scene_normals_->points.size() != scene_->points.size()))
        computeNormals<PointT>(scene_, scene_normals_, param_.normal_computation_method_);

    typename std::map<std::string, ObjectHypothesis<PointT> >::iterator it;
    for (it = obj_hypotheses_.begin (); it != obj_hypotheses_.end (); it++)
    {
        ObjectHypothesis<PointT> &oh = it->second;

        if(oh.model_scene_corresp_.size() < 3)
            continue;

        std::vector < pcl::Correspondences > corresp_clusters;
        cg_algorithm_->setSceneCloud (scene_);
        cg_algorithm_->setInputCloud (oh.model_->keypoints_);

        if(cg_algorithm_->getRequiresNormals())
            cg_algorithm_->setInputAndSceneNormals(oh.model_->kp_normals_, scene_normals_);

        //we need to pass the keypoints_pointcloud and the specific object hypothesis
        cg_algorithm_->setModelSceneCorrespondences (oh.model_scene_corresp_);
        cg_algorithm_->cluster (corresp_clusters);

        std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > new_transforms (corresp_clusters.size());
        typename pcl::registration::TransformationEstimationSVD < PointT, PointT > t_est;

        for (size_t i = 0; i < corresp_clusters.size(); i++)
            t_est.estimateRigidTransformation (*oh.model_->keypoints_, *scene_, corresp_clusters[i], new_transforms[i]);

        if(param_.merge_close_hypotheses_) {
            std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > merged_transforms (corresp_clusters.size());
            std::vector<bool> cluster_has_been_taken(corresp_clusters.size(), false);
            const double angle_thresh_rad = param_.merge_close_hypotheses_angle_ * M_PI / 180.f ;

            size_t kept=0;
            for (size_t i = 0; i < new_transforms.size(); i++) {

                if (cluster_has_been_taken[i])
                    continue;

                cluster_has_been_taken[i] = true;
                const Eigen::Vector3f centroid1 = new_transforms[i].block<3, 1> (0, 3);
                const Eigen::Matrix3f rot1 = new_transforms[i].block<3, 3> (0, 0);

                pcl::Correspondences merged_corrs = corresp_clusters[i];

                for(size_t j=i; j < new_transforms.size(); j++) {
                    const Eigen::Vector3f centroid2 = new_transforms[j].block<3, 1> (0, 3);
                    const Eigen::Matrix3f rot2 = new_transforms[j].block<3, 3> (0, 0);
                    const Eigen::Matrix3f rot_diff = rot2 * rot1.transpose();

                    double rotx = atan2(rot_diff(2,1), rot_diff(2,2));
                    double roty = atan2(-rot_diff(2,0), sqrt(rot_diff(2,1) * rot_diff(2,1) + rot_diff(2,2) * rot_diff(2,2)));
                    double rotz = atan2(rot_diff(1,0), rot_diff(0,0));
                    double dist = (centroid1 - centroid2).norm();

                    if ( (dist < param_.merge_close_hypotheses_dist_) && (rotx < angle_thresh_rad) && (roty < angle_thresh_rad) && (rotz < angle_thresh_rad) ) {
                        merged_corrs.insert( merged_corrs.end(), corresp_clusters[j].begin(), corresp_clusters[j].end() );
                        cluster_has_been_taken[j] = true;
                    }
                }

                t_est.estimateRigidTransformation (*oh.model_->keypoints_, *scene_, merged_corrs, merged_transforms[kept]);
                kept++;
            }
            merged_transforms.resize(kept);
            new_transforms = merged_transforms;
        }

        std::cout << "Merged " << corresp_clusters.size() << " clusters into " << new_transforms.size() << " clusters. Total correspondences: " << oh.model_scene_corresp_.size () << " " << it->first << std::endl;

        //        oh.visualize(*scene_);

        size_t existing_hypotheses = models_.size();
        models_.resize( existing_hypotheses + new_transforms.size(), oh.model_  );
        transforms_.insert(transforms_.end(), new_transforms.begin(), new_transforms.end());
    }
}
template<template<class > class Distance, typename PointT, typename FeatureT> bool
GlobalNNPipelineROS<Distance,PointT,FeatureT>::classifyROS (classifier_srv_definitions::classify::Request & req, classifier_srv_definitions::classify::Response & response)
    {
        typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>());
        pcl::fromROSMsg(req.cloud, *cloud);
        this->input_ = cloud;

        pcl::PointCloud<pcl::PointXYZRGB>::Ptr pClusteredPCl (new pcl::PointCloud<pcl::PointXYZRGB>());
        pcl::copyPointCloud(*cloud, *pClusteredPCl);

        //clear all data from previous visualization steps and publish empty markers/point cloud
        for (size_t i=0; i < markerArray_.markers.size(); i++)
        {
            markerArray_.markers[i].action = visualization_msgs::Marker::DELETE;
        }
        vis_pub_.publish( markerArray_ );
        sensor_msgs::PointCloud2 scenePc2;
        vis_pc_pub_.publish(scenePc2);
        markerArray_.markers.clear();

        for(size_t cluster_id=0; cluster_id < req.clusters_indices.size(); cluster_id++)
        {
          std::vector<int> cluster_indices_int;
          Eigen::Vector4f centroid;
          Eigen::Matrix3f covariance_matrix;

          const float r = std::rand() % 255;
          const float g = std::rand() % 255;
          const float b = std::rand() % 255;

          this->indices_.resize(req.clusters_indices[cluster_id].data.size());
          for(size_t kk=0; kk < req.clusters_indices[cluster_id].data.size(); kk++)
          {
                this->indices_[kk] = static_cast<int>(req.clusters_indices[cluster_id].data[kk]);
                pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).r = 0.8*r;
                pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).g = 0.8*g;
                pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).b = 0.8*b;
          }

          this->classify ();

          std::cout << "for cluster " << cluster_id << " with size " << cluster_indices_int.size() << ", I have following hypotheses: " << std::endl;

          object_perception_msgs::classification class_tmp;
          for(size_t kk=0; kk < this->categories_.size(); kk++)
          {
            std::cout << this->categories_[kk] << " with confidence " << this->confidences_[kk] << std::endl;
            std_msgs::String str_tmp;
            str_tmp.data = this->categories_[kk];
            class_tmp.class_type.push_back(str_tmp);
            class_tmp.confidence.push_back( this->confidences_[kk] );
          }
          response.class_results.push_back(class_tmp);

          typename pcl::PointCloud<PointT>::Ptr pClusterPCl_transformed (new pcl::PointCloud<PointT>());
          pcl::computeMeanAndCovarianceMatrix(*this->input_, cluster_indices_int, covariance_matrix, centroid);
          Eigen::Matrix3f eigvects;
          Eigen::Vector3f eigvals;
          pcl::eigen33(covariance_matrix, eigvects,  eigvals);

          Eigen::Vector3f centroid_transformed = eigvects.transpose() * centroid.topRows(3);

          Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Zero(4,4);
          transformation_matrix.block<3,3>(0,0) = eigvects.transpose();
          transformation_matrix.block<3,1>(0,3) = -centroid_transformed;
          transformation_matrix(3,3) = 1;

          pcl::transformPointCloud(*this->input_, cluster_indices_int, *pClusterPCl_transformed, transformation_matrix);

          //pcl::transformPointCloud(*frame_, cluster_indices_int, *frame_eigencoordinates_, eigvects);
          PointT min_pt, max_pt;
          pcl::getMinMax3D(*pClusterPCl_transformed, min_pt, max_pt);
          std::cout << "Elongations along eigenvectors: " << max_pt.x - min_pt.x << ", " << max_pt.y - min_pt.y
                    << ", " << max_pt.z - min_pt.z << std::endl;
          geometry_msgs::Point32 centroid_ros;
          centroid_ros.x = centroid[0];
          centroid_ros.y = centroid[1];
          centroid_ros.z = centroid[2];
          response.centroid.push_back(centroid_ros);

          // calculating the bounding box of the cluster
          Eigen::Vector4f min;
          Eigen::Vector4f max;
          pcl::getMinMax3D (*this->input_, cluster_indices_int, min, max);

          object_perception_msgs::BBox bbox;
          geometry_msgs::Point32 pt;
          pt.x = min[0]; pt.y = min[1]; pt.z = min[2]; bbox.point.push_back(pt);
          pt.x = min[0]; pt.y = min[1]; pt.z = max[2]; bbox.point.push_back(pt);
          pt.x = min[0]; pt.y = max[1]; pt.z = min[2]; bbox.point.push_back(pt);
          pt.x = min[0]; pt.y = max[1]; pt.z = max[2]; bbox.point.push_back(pt);
          pt.x = max[0]; pt.y = min[1]; pt.z = min[2]; bbox.point.push_back(pt);
          pt.x = max[0]; pt.y = min[1]; pt.z = max[2]; bbox.point.push_back(pt);
          pt.x = max[0]; pt.y = max[1]; pt.z = min[2]; bbox.point.push_back(pt);
          pt.x = max[0]; pt.y = max[1]; pt.z = max[2]; bbox.point.push_back(pt);
          response.bbox.push_back(bbox);

          // getting the point cloud of the cluster
          typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
          pcl::copyPointCloud(*this->input_, cluster_indices_int, *cluster);

          sensor_msgs::PointCloud2  pc2;
          pcl::toROSMsg (*cluster, pc2);
          response.cloud.push_back(pc2);

          // visualize the result as ROS topic
            visualization_msgs::Marker marker;
            marker.header.frame_id = camera_frame_;
            marker.header.stamp = ros::Time::now();
            //marker.header.seq = ++marker_seq_;
            marker.ns = "object_classification";
            marker.id = cluster_id;
            marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
            marker.action = visualization_msgs::Marker::ADD;
            marker.pose.position.x = centroid_ros.x;
            marker.pose.position.y = centroid_ros.y - 0.1;
            marker.pose.position.z = centroid_ros.z - 0.1;
            marker.pose.orientation.x = 0;
            marker.pose.orientation.y = 0;
            marker.pose.orientation.z = 0;
            marker.pose.orientation.w = 1.0;
            marker.scale.z = 0.1;
            marker.color.a = 1.0;
            marker.color.r = r/255.f;
            marker.color.g = g/255.f;
            marker.color.b = b/255.f;
            std::stringstream marker_text;
            marker_text.precision(2);
            marker_text << this->categories_[0] << this->confidences_[0];
            marker.text = marker_text.str();
            markerArray_.markers.push_back(marker);
        }

        pcl::toROSMsg (*pClusteredPCl, scenePc2);
        vis_pc_pub_.publish(scenePc2);
        vis_pub_.publish( markerArray_ );

        response.clusters_indices = req.clusters_indices;
        return true;
    }
void
NMBasedCloudIntegration<PointT>::collectInfo ()
{
  size_t total_point_count = 0;
  for(size_t i = 0; i < input_clouds_.size(); i++)
    total_point_count += (indices_.empty() || indices_[i].empty()) ? input_clouds_[i]->size() : indices_[i].size();
  VLOG(1) << "Allocating memory for point information of " << total_point_count << "points. ";
  big_cloud_info_.resize(total_point_count);


  std::vector<pcl::PointCloud<PointT> > input_clouds_aligned (input_clouds_.size());
  std::vector<pcl::PointCloud<pcl::Normal> > input_normals_aligned (input_clouds_.size());

#pragma omp parallel for schedule(dynamic)
  for(size_t i=0; i < input_clouds_.size(); i++)
  {
      pcl::transformPointCloud(*input_clouds_[i], input_clouds_aligned[i], transformations_to_global_[i]);
      transformNormals(*input_normals_[i], input_normals_aligned[i], transformations_to_global_[i]);
  }

  size_t point_count = 0;
  for(size_t i=0; i < input_clouds_.size(); i++)
  {
    const pcl::PointCloud<PointT> &cloud_aligned = input_clouds_aligned[i];
    const pcl::PointCloud<pcl::Normal> &normals_aligned = input_normals_aligned[i];

    size_t kept_new_pts = 0;
    if (indices_.empty() || indices_[i].empty())
    {
      for(size_t jj=0; jj<cloud_aligned.points.size(); jj++)
      {
        if ( !pcl::isFinite(cloud_aligned.points[jj]) || !pcl::isFinite(normals_aligned.points[jj]) )
          continue;

        PointInfo &pt = big_cloud_info_[point_count + kept_new_pts];
        pt.pt = cloud_aligned.points[jj];
        pt.normal = normals_aligned.points[jj];
        pt.sigma_lateral = pt_properties_[i][jj][0];
        pt.sigma_axial = pt_properties_[i][jj][1];
        pt.distance_to_depth_discontinuity = pt_properties_[i][jj][2];
        pt.pt_idx = jj;
        kept_new_pts++;
      }
    }
    else
    {
      for(int idx : indices_[i])
      {
        if ( !pcl::isFinite(cloud_aligned.points[idx]) || !pcl::isFinite(normals_aligned.points[idx]) )
           continue;

        PointInfo &pt = big_cloud_info_[point_count + kept_new_pts];
        pt.pt = cloud_aligned.points[idx];
        pt.normal = normals_aligned.points[ idx ];
        pt.sigma_lateral = pt_properties_[i][idx][0];
        pt.sigma_axial = pt_properties_[i][idx][1];
        pt.distance_to_depth_discontinuity = pt_properties_[i][idx][2];
        pt.pt_idx = idx;
        kept_new_pts++;
      }
    }

    // compute and store remaining information
#pragma omp parallel for schedule (dynamic) firstprivate(i, point_count, kept_new_pts)
    for(size_t jj=0; jj<kept_new_pts; jj++)
    {
      PointInfo &pt = big_cloud_info_ [point_count + jj];
      pt.origin = i;

      Eigen::Matrix3f sigma = Eigen::Matrix3f::Zero(), sigma_aligned = Eigen::Matrix3f::Zero();
      sigma(0,0) = pt.sigma_lateral;
      sigma(1,1) = pt.sigma_lateral;
      sigma(2,2) = pt.sigma_axial;

      const Eigen::Matrix4f &tf = transformations_to_global_[ i ];
      Eigen::Matrix3f rotation = tf.block<3,3>(0,0); // or inverse?
      sigma_aligned = rotation * sigma * rotation.transpose();
      double det = sigma_aligned.determinant();

//      if( std::isfinite(det) && det>0)
//          pt.probability = 1 / sqrt(2 * M_PI * det);
//      else
//          pt.probability = std::numeric_limits<float>::min();

      if( std::isfinite(det) && det>0)
          pt.weight = det;
      else
          pt.weight = std::numeric_limits<float>::max();
    }

    point_count += kept_new_pts;
  }

  big_cloud_info_.resize(point_count);
}
Beispiel #25
0
int
main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>);

	if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("../learn15.pcd", *cloud) == -1) //* load the file
	{
		PCL_ERROR("Couldn't read file test_pcd.pcd \n");
		return (-1);
	}

	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	// Create the segmentation object
	pcl::SACSegmentation<pcl::PointXYZRGB> seg;
	// Optional
	seg.setOptimizeCoefficients(true);
	// Mandatory
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setDistanceThreshold(0.01);
	seg.setInputCloud(cloud);
	seg.segment(*inliers, *coefficients);

	if (inliers->indices.size() == 0)
	{
		PCL_ERROR("Could not estimate a planar model for the given dataset.");
		return (-1);
	}

	std::cerr << "Model coefficients: " << coefficients->values[0] << " "
		<< coefficients->values[1] << " "
		<< coefficients->values[2] << " "
		<< coefficients->values[3] << std::endl;

	pcl::ExtractIndices<pcl::PointXYZRGB> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(inliers);
	extract.setNegative(true);
	//Removes part_of_cloud but retain the original full_cloud
	//extract.setNegative(true); // Removes part_of_cloud from full cloud  and keep the rest
	extract.filter(*cloud_plane);

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
	sor.setInputCloud(cloud_plane);
	sor.setMeanK(50);
	sor.setStddevMulThresh(1.0);
	sor.filter(*cloud_filtered);

	//cloud.swap(cloud_plane);
	/*
	pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
	viewer.showCloud(cloud_plane);
	while (!viewer.wasStopped())
	{
	}
	*/
	Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
	Eigen::Matrix<float, 1, 3> fitted_plane_norm, xyaxis_plane_norm, rotation_axis;
	fitted_plane_norm[0] = coefficients->values[0];
	fitted_plane_norm[1] = coefficients->values[1];
	fitted_plane_norm[2] = coefficients->values[2];
	xyaxis_plane_norm[0] = 0.0;
	xyaxis_plane_norm[1] = 0.0;
	xyaxis_plane_norm[2] = 1.0;
	rotation_axis = xyaxis_plane_norm.cross(fitted_plane_norm);
	float theta = acos(xyaxis_plane_norm.dot(fitted_plane_norm)); 
	//float theta = -atan2(rotation_axis.norm(), xyaxis_plane_norm.dot(fitted_plane_norm));
	transform_2.rotate(Eigen::AngleAxisf(theta, rotation_axis));
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_recentered(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::transformPointCloud(*cloud_filtered, *cloud_recentered, transform_2);

	
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>); 
	pcl::copyPointCloud(*cloud_recentered, *cloud_xyz);

	///////////////////////////////////////////////////////////////////
	Eigen::Vector4f pcaCentroid;
	pcl::compute3DCentroid(*cloud_xyz, pcaCentroid);
 	Eigen::Matrix3f covariance;
	
	computeCovarianceMatrixNormalized(*cloud_xyz, pcaCentroid, covariance);
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
	Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
	std::cout << eigenVectorsPCA.size() << std::endl;
	if(eigenVectorsPCA.size()!=9)
	eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));


	
	Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity());
	projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();
	projectionTransform.block<3, 1>(0, 3) = -1.f * (projectionTransform.block<3, 3>(0, 0) * pcaCentroid.head<3>());
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointsProjected(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*cloud_xyz, *cloudPointsProjected, projectionTransform);
	// Get the minimum and maximum points of the transformed cloud.
	pcl::PointXYZ minPoint, maxPoint;
	pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);
	const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap());



	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	viewer = rgbVis(cloud);
	// viewer->addPointCloud<pcl::PointXYZ>(cloudPointsProjected, "Simple Cloud");
	// viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "Simple Cloud2");
	viewer->addCube(minPoint.x, maxPoint.x, minPoint.y, maxPoint.y, minPoint.z , maxPoint.z);

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	
	/*
	pcl::PCA< pcl::PointXYZ > pca;
	pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud;
	pcl::PointCloud< pcl::PointXYZ > proj;

	pca.setInputCloud(cloud);
	pca.project(*cloud, proj);

	Eigen::Vector4f proj_min;
	Eigen::Vector4f proj_max;
	pcl::getMinMax3D(proj, proj_min, proj_max);

	pcl::PointXYZ min;
	pcl::PointXYZ max;
	pca.reconstruct(proj_min, min);
	pca.reconstruct(proj_max, max);

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	viewer->addCube(min.x, max.x, min.y, max.y, min.z, max.z);

	*/


	return (0);

}
Beispiel #26
0
int main(int argc, char** argv)
{

// Punktwolke laden
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_unfiltered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile <pcl::PointXYZ> ("Arena_24_7_2014.pcd",*cloud_unfiltered) == -1)
{
std::cout<<"Cloud reading failed"<<std::endl;
return (-1);
}

pcl::PCDWriter writer;

//Filter für Höhen- und Tiefenbegrenzung:
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud_unfiltered);
pass.setFilterFieldName("z");
pass.setFilterLimits(-0.2,0.3);//Für Stativ ca. (-0.9,-0.2)
pass.filter(*cloud);

// Berechnen der Punktnormalen
pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod(tree);
normal_estimator.setInputCloud(cloud);
normal_estimator.setKSearch(100);// Ursprünglich:50
// http://pointclouds.org/documentation/tutorials/normal_estimation.php
normal_estimator.compute(*normals);

// Segmentierung mit dem RegionGrowing-Algorithmus (Setzen der Parameter)
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
reg.setMinClusterSize (200);
//reg.setMaxClusterSize (10000);
reg.setSearchMethod (tree);
reg.setNumberOfNeighbours (50); // Ursprünglich 50 (Bei ca. 150 werden die Kanten schön glatt!)
// aber es wird nach zu vielen Nachbarn gecheckt-> Mehrere Banden fallen in ein Cluster
reg.setInputCloud (cloud);
//reg.setIndices (indices);
reg.setInputNormals (normals);
reg.setSmoothnessThreshold (4.0 / 180.0 * M_PI); // Ursprünglich: 7.0/180*M_PI
// je kleiner desto weniger punkte sind "smooth" genug ->Klares Abgrenzen der Cluster!
reg.setCurvatureThreshold (.3);//Ursprünglich:1.0 // je kleiner desto geringer darf sich das Cluster krümmen

// Anwendung des Cluster-Filters auf Input-Wolke "cloud"
std::vector <pcl::PointIndices> clusters;
reg.extract (clusters);

pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr planes_cloud (new pcl::PointCloud<pcl::PointXYZ>);

//vtkSmartPointer<vtkAppendPolyData> appendFilter =
//vtkSmartPointer<vtkAppendPolyData>::New();
//vtkSmartPointer<vtkPolyData> polyplane =
//vtkSmartPointer<vtkPolyData>::New();

int b=0;

// Schleife über alle detektierten Cluster
for(int a=0;a<clusters.size();a++)
{
if(clusters[a].indices.size() >100 )
{
pcl::IndicesPtr indices_ptr (new std::vector<int> (clusters[a].indices.size ()));
for (int i=0;i<indices_ptr->size();i++)
{
(*indices_ptr)[i]=clusters[a].indices[i]; // http://www.pcl-users.org/Removing-a-cluster-Problem-with-pointer-td4023699.html
} // Indizes des jeweiligen Clusters werden alle in indices_ptr gespeichert
// Punkte des Clusters werden in cluster_cloud geschrieben
extract.setIndices(indices_ptr);
extract.setNegative(false);
extract.filter(*cluster_cloud);// Punkte des Cluster a werden in cluster_cloud geschrieben
std::cout<<"cluster_cloud["<<a<<"] hat "<<cluster_cloud->width*cluster_cloud->height<<" Punkte."<<std::endl;

//Erzeugen einer Ebene aus Cluster_cloud
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(0.1);
seg.setInputCloud(cluster_cloud);
seg.segment(*inliers, *coefficients);

// Wenn Ebene vertikal: Abspeichern in Cluster_i.pcd
if(coefficients->values[2]<.9 && coefficients->values[2]>(-.9))//ax+by+cz+d=0 (wenn c=0 => Ebene parallel zur z-Achse)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr planes_projected (new pcl::PointCloud<pcl::PointXYZ>);
//Inliers auf Ebene projizieren:
pcl::PCA<pcl::PointXYZ> pca2;
pcl::ProjectInliers<pcl::PointXYZ> proj2;
proj2.setModelType(pcl::SACMODEL_PLANE);
proj2.setIndices(inliers);
proj2.setInputCloud(cluster_cloud);
proj2.setModelCoefficients(coefficients);
proj2.filter(*planes_projected);

// Punkte in Eigenraum transformieren, um bounding box zu berechnen (Quelle: pcl-users Forum (http://www.pcl-users.org/Finding-oriented-bounding-box-of-a-cloud-td4024616.html)
// compute principal direction
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*planes_projected, centroid);
Eigen::Matrix3f covariance;
computeCovarianceMatrixNormalized(*planes_projected, centroid, covariance);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
Eigen::Matrix3f eigDx = eigen_solver.eigenvectors();
eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1));

// move the points to the that reference frame
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<pcl::PointXYZ> cPoints;
pcl::transformPointCloud(*planes_projected, cPoints, p2w);
pcl::PointXYZ min_pt, max_pt;
pcl::getMinMax3D(cPoints, min_pt, max_pt);
const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());

// final transform
const Eigen::Quaternionf qfinal(eigDx);
const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>();

// Punktwolke und bounding box im viewer anzeigen
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer);
viewer->addCoordinateSystem ();
viewer->addPointCloud(planes_projected);// Danach auskommentieren
viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z);


// Ausgabe der Eckpunkte (kann gelöscht werden)
std::cout << " min.x= " << min_pt.x << " max.x= " << max_pt.x << " min.y= " << min_pt.y << " max.y= " << max_pt.y << " min.z= " << min_pt.z << " max.z= " << max_pt.z<< std::endl;
std::cout << "Punkte: " << min_pt.x <<";" << min_pt.y << ";" << min_pt.z <<std::endl;
std::cout << min_pt.x <<";" << min_pt.y << ";" << max_pt.z <<std::endl;
std::cout << min_pt.x <<";" << max_pt.y << ";" << min_pt.z <<std::endl;
std::cout << min_pt.x <<";" << max_pt.y << ";" << max_pt.z <<std::endl;
std::cout << max_pt.x <<";" << min_pt.y << ";" << min_pt.z <<std::endl;
std::cout << max_pt.x <<";" << min_pt.y << ";" << max_pt.z <<std::endl;
std::cout << max_pt.x <<";" << max_pt.y << ";" << min_pt.z <<std::endl;
std::cout << max_pt.x <<";" << max_pt.y << ";" << max_pt.z <<std::endl;

// Erzeugen einer Punktwolke mit Punkten, die zwischen den Extrema liegen (um anschließend eine dichte Oberfläche zu erzeugen)
pcl::PointCloud<pcl::PointXYZ>::Ptr Fillcloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointXYZ P1,P2,P3,P4;
P1.x=max_pt.x;P2.x=max_pt.x; P3.x=max_pt.x; P4.x=max_pt.x;
P1.y=min_pt.y;P2.y=min_pt.y; P3.y=max_pt.y; P4.y=max_pt.y;
P1.z=min_pt.z;P2.z=max_pt.z; P3.z=min_pt.z; P4.z=max_pt.z;

//P4.x=min_pt.x;P3.x=min_pt.x; P2.x=min_pt.x; P1.x=min_pt.x;
//P4.y=min_pt.y;P3.y=min_pt.y; P2.y=max_pt.y; P1.y=max_pt.y;
//P4.z=min_pt.z;P3.z=max_pt.z; P2.z=min_pt.z; P1.z=max_pt.z;

Fillcloud->push_back(P1);
Fillcloud->push_back(P2);
Fillcloud->push_back(P3);
Fillcloud->push_back(P4);
// Fillcloud->push_back (pcl::PointXYZ (min_pt.x, min_pt.y, min_pt.z)); // P1
// Fillcloud->push_back (pcl::PointXYZ (min_pt.x, min_pt.y, max_pt.z)); // P2
// Fillcloud->push_back (pcl::PointXYZ (min_pt.x, max_pt.y, min_pt.z)); // P3
// Fillcloud->push_back (pcl::PointXYZ (min_pt.x, max_pt.y, max_pt.z)); // P4 // Enweder alle x_max oder alle x_min nehmen WARUM?????
// Fillcloud->push_back (pcl::PointXYZ (max_pt.x, min_pt.y, min_pt.z));
// Fillcloud->push_back (pcl::PointXYZ (max_pt.x, min_pt.y, max_pt.z));
// Fillcloud->push_back (pcl::PointXYZ (max_pt.x, max_pt.y, min_pt.z));
// Fillcloud->push_back (pcl::PointXYZ (max_pt.x, max_pt.y, max_pt.z));
// Schleife, um BoundingBox-"Fläche" mit Punkten zu füllen (um ein Mesh erzeugen zu können


int AnzahlPunktehoch = 80;
int AnzahlPunktebreit = 400;
for( int ii = 0; ii < AnzahlPunktebreit+1; ii++ ){
    Fillcloud->push_back( pcl::PointXYZ (P1.x+((P2.x-P1.x)/AnzahlPunktebreit)*ii , P1.y+((P2.y-P1.y)/AnzahlPunktebreit)*ii , P1.z+((P2.z-P1.z)/AnzahlPunktebreit)*ii) );
    for( int jj = 0; jj < AnzahlPunktehoch+1; jj++ ){
        Fillcloud->push_back( pcl::PointXYZ (P1.x+((P2.x-P1.x)/AnzahlPunktebreit)*ii + (P3.x-P1.x)/AnzahlPunktehoch*jj , P1.y+((P2.y-P1.y)/AnzahlPunktebreit)*ii + (P3.y-P1.y)/AnzahlPunktehoch*jj , P1.z+((P2.z-P1.z)/AnzahlPunktebreit)*ii + (P3.z-P1.z)/AnzahlPunktehoch*jj));
    }
}

// Erzeugte Punktwolke, die die Extrema "verbindet" rücktransformieren
pcl::PointCloud<pcl::PointXYZ>::Ptr Fillcloud_transformiert (new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*Fillcloud,*Fillcloud_transformiert,tfinal,qfinal);

// Fillcloud wird zum Viewer hinzugefügt; "spin" auskommentieren wenn nicht jedes Fenster einzeln weggeklickt werden soll
//viewer->addPointCloud(Fillcloud_transformiert);
//viewer->spin();

//Alle Clusterebenen, die vertikal sind werden in planes_cloud gespeichert
*planes_cloud+=*Fillcloud_transformiert;

// Aus aktuellen Punkten vtkPlaneSource erzeugen
vtkPlaneSource *plane= vtkPlaneSource::New();
vtkPlaneSource *plane2= vtkPlaneSource::New();
plane->SetOrigin(min_pt.x,min_pt.y,min_pt.z);
plane->SetPoint1(min_pt.x,min_pt.y,max_pt.z);
plane->SetPoint2(min_pt.x,max_pt.y,min_pt.z);
plane->SetXResolution(10);
//plane->SetResolution(10,200); // Set the number of x-y subdivisions in the plane
plane->Update();
plane2->SetOrigin(min_pt.x,min_pt.y,max_pt.z);
plane2->SetPoint1(min_pt.x,max_pt.y,max_pt.z);
plane2->SetPoint2(min_pt.x,max_pt.y,min_pt.z);
//plane->SetResolution(1,1);
plane2->Update();
// appender->AddInputConnection(plane->GetOutputPort());
// appender->Update();
// actor->SetMapper(mapper);
// // Aktuelle Plane zu PlaneCollection hinzufügen
// PlaneCollection->AddItem(plane->GetOutputPort());
// PlaneCollection->AddItem(plane);
// mapper->SetInputConnection(plane->GetOutputPort());
// plane->Delete();
// mapper->SetInputConnection(plane->GetOutputPort());
// actor->SetMapper(mapper);
// renderer->AddActor(plane);
// vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
// polydata->SetVerts();
// polyplane->ShallowCopy(plane->GetOutput());
// appendFilter->AddInputConnection(polyplane->GetProducerPort());

//appendFilter->AddInput(plane->GetOutput());
//appendFilter->Update();

//appendFilter->AddInput(plane2->GetOutput());
//appendFilter->Update();
b=b+1;
}
}
}
// PlanesCollection als STL abspeichern
//vtkSmartPointer<vtkSTLWriter> schreiber = vtkSmartPointer<vtkSTLWriter>::New();
//schreiber->SetInput(appendFilter->GetOutput());
//schreiber->SetFileName("funktioniert nicht!");
//schreiber->Update();
//schreiber->Write();

// schreiber->SetInputConnection(plane->GetOutputPort());
// schreiber->SetFileName("");
// schreiber->SetInput(appender->GetOutput());
// //schreiber->SetFileTypeToASCII();
// schreiber->Write();

// Ausgabe
std::cout<<"Es wurden "<<b<<" Flächen in Planes_cloud.pcd geschrieben"<<endl;
writer.write("Planes_cloud.pcd",*planes_cloud,false);
std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;
std::cout << "First cluster has " << clusters[0].indices.size () << " points." << endl;
std::cout << "These are the indices of the points of the initial" <<
std::endl << "cloud that belong to the first cluster:" << std::endl;
int counter = 0;
while (counter < 5 || counter > clusters[0].indices.size ())
{
std::cout << clusters[0].indices[counter] << std::endl;
counter++;
}

// planes_cloud zu einer Oberfläche konvertieren u. als stl oder vtk speichern!
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals_triangles (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_triangles (new pcl::search::KdTree<pcl::PointXYZ>);
tree_triangles->setInputCloud(planes_cloud);
n.setInputCloud(planes_cloud);
n.setSearchMethod(tree_triangles);
n.setKSearch(20);
n.compute(*normals_triangles);

pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields (*planes_cloud,*normals_triangles,*cloud_with_normals);
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud(cloud_with_normals);
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
pcl::PolygonMesh triangles;

gp3.setSearchRadius(0.4);// Ursprünglich 0.025
gp3.setMu(5.0); // Ursprünglich 2.5
gp3.setMaximumNearestNeighbors(20); // davor 100
gp3.setMaximumSurfaceAngle(M_PI/4); // ursprünglich: M_PI/4
gp3.setMinimumAngle(M_PI/18);
gp3.setMaximumAngle(2*M_PI/1); // ursprunglich: 2*M_PI/3
gp3.setNormalConsistency(false);
gp3.setInputCloud(cloud_with_normals);
gp3.setSearchMethod(tree2);
gp3.reconstruct(triangles);

std::vector<int> parts = gp3.getPartIDs();
std::vector<int> states = gp3.getPointStates();

// Oberfläche als STL-Modell abspeichern
pcl::io::savePolygonFileSTL("Arena_24_7_2014.stl", triangles);
//pcl::io::saveVTKFile("mesh.vtk", triangles);

// Farbliche Visualisierung der segmentierten Punktwolke
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
pcl::visualization::CloudViewer viewer ("Cluster viewer");
viewer.showCloud(colored_cloud);
viewer.runOnVisualizationThreadOnce(background_color);
while (!viewer.wasStopped ())
{
}
/*
pcl::PointCloud <pcl::PointXYZ>::Ptr cluster_cloud=clusters[0].indices;
pcl::visualization::CloudViewer viewer ("Cluster viewer");
viewer.showCloud(cluster_cloud);
while (!viewer.wasStopped())
{
}
*/
return (0);
}
Beispiel #27
0
  void PwnTracker::processFrame(const DepthImage& depthImage_, 
				const Eigen::Isometry3f& sensorOffset_, 
				const Eigen::Matrix3f& cameraMatrix_,
				const Eigen::Isometry3f& initialGuess){
    int r,c;
    Eigen::Matrix3f scaledCameraMatrix = cameraMatrix_;
    _currentCloudOffset = sensorOffset_;
    _currentCameraMatrix = cameraMatrix_;
    _currentDepthImage = depthImage_;
    _currentCloud = makeCloud(r,c,scaledCameraMatrix,_currentCloudOffset,_currentDepthImage);
    
    bool newFrame = false;
    bool newRelation = false;
    PwnTrackerFrame* currentTrackerFrame=0;
    Eigen::Isometry3d relationMean;
    if (_previousCloud){
      _aligner->setCurrentSensorOffset(_currentCloudOffset);
      _aligner->setCurrentFrame(_currentCloud);
      _aligner->setReferenceSensorOffset(_previousCloudOffset);
      _aligner->setReferenceFrame(_previousCloud);

      _aligner->correspondenceFinder()->setImageSize(r,c);
      PinholePointProjector* alprojector=dynamic_cast<PinholePointProjector*>(_aligner->projector());
      alprojector->setCameraMatrix(scaledCameraMatrix);
      alprojector->setImageSize(r,c);

      Eigen::Isometry3f guess=_previousCloudTransform.inverse()*_globalT*initialGuess;
      _aligner->setInitialGuess(guess);
      double t0, t1;
      t0 = g2o::get_time();
      _aligner->align();

      t1 = g2o::get_time();
      std::cout << "Time: " << t1 - t0 << " seconds " << std::endl;
 
      cerr << "inliers: " << _aligner->inliers() << endl;
      // cerr << "chi2: " << _aligner->error() << endl;
      // cerr << "chi2/inliers: " << _aligner->error()/_aligner->inliers() << endl;
      // cerr << "initialGuess: " << t2v(guess).transpose() << endl;
      // cerr << "transform   : " << t2v(_aligner->T()).transpose() << endl;
      if (_aligner->inliers()>0){
    	_globalT = _previousCloudTransform*_aligner->T();
    	//cerr << "TRANSFORM FOUND" <<  endl;
      } else {
    	//cerr << "FAILURE" <<  endl;
    	_globalT = _globalT*guess;
      }
      convertScalar(relationMean, _aligner->T());
      if (! (_counter%50) ) {
    	Eigen::Matrix3f R = _globalT.linear();
    	Eigen::Matrix3f E = R.transpose() * R;
    	E.diagonal().array() -= 1;
    	_globalT.linear() -= 0.5 * R * E;
      }
      _globalT.matrix().row(3) << 0,0,0,1;
 
      newAlignmentCallbacks(_globalT, _aligner->T(), _aligner->inliers(), _aligner->error());
			   
      int maxInliers = r*c;
      float inliersFraction = (float) _aligner->inliers()/(float) maxInliers;
      cerr << "inliers/maxinliers/fraction: " << _aligner->inliers() << "/" << maxInliers << "/" << inliersFraction << endl;
      if (inliersFraction<_newFrameInliersFraction){
	newFrame = true;
	newRelation = true;

	// char filename[1024];
	// sprintf (filename, "frame-%05d.pwn", _numKeyframes);
	// frame->save(filename,1,true,_globalT);

	_numKeyframes ++;
	if (!_cache) 
	  delete _previousCloud;
	else{
	  _previousCloudHandle.release();
	}
	//_cache->unlock(_previousTrackerFrame);
	// _aligner->setReferenceSensorOffset(_currentCloudOffset);
	// _aligner->setReferenceFrame(_currentCloud);
	_previousCloud = _currentCloud;
	_previousCloudTransform = _globalT;
	// cerr << "new frame added (" << _numKeyframes <<  ")" << endl;
	// cerr << "inliers: " << _aligner->inliers() << endl;
	// cerr << "maxInliers: " << maxInliers << endl;
	// cerr << "chi2: " << _aligner->error() << endl;
	// cerr << "chi2/inliers: " << _aligner->error()/_aligner->inliers() << endl;
	// cerr << "initialGuess: " << t2v(guess).transpose() << endl;
	// cerr << "transform   : " << t2v(_aligner->T()).transpose() << endl;
	// cerr << "globalTransform   : " << t2v(_globalT).transpose() << endl;
      } else { // previous frame but offset is small
	delete _currentCloud;
	_currentCloud = 0;
      }
    } else { // first frame
      //ser.writeObject(*manager);
      newFrame = true;
      // _aligner->setReferenceSensorOffset(_currentCloudOffset);
      // _aligner->setReferenceFrame(_currentCloud);
      _previousCloud = _currentCloud;
      _previousCloudTransform = _globalT;
      _previousCloudOffset = _currentCloudOffset;
      _numKeyframes ++;
      /*Eigen::Isometry3f t = _globalT;
	geometry_msgs::Point p;
	p.x = t.translation().x();
	p.y = t.translation().y();
	p.z = t.translation().z();
	m_odometry.points.push_back(p);
      */
    }
    _counter++;

    if (newFrame) {
      //cerr << "maing new frame, previous: " << _previousTrackerFrame << endl;
      currentTrackerFrame = new PwnTrackerFrame(_manager);
      //currentTrackerFrame->cloud.set(cloud);
      currentTrackerFrame->cloud=_currentCloud;
      currentTrackerFrame->sensorOffset = _currentCloudOffset;
      boss_map::ImageBLOB* depthBLOB=new boss_map::ImageBLOB();
      DepthImage_convert_32FC1_to_16UC1(depthBLOB->cvImage(),_currentDepthImage);
      //depthImage.toCvMat(depthBLOB->cvImage());
      depthBLOB->adjustFormat();
      currentTrackerFrame->depthImage.set(depthBLOB);
      
      boss_map::ImageBLOB* normalThumbnailBLOB=new boss_map::ImageBLOB();
      boss_map::ImageBLOB* depthThumbnailBLOB=new boss_map::ImageBLOB();
      makeThumbnails(depthThumbnailBLOB->cvImage(), normalThumbnailBLOB->cvImage(), _currentCloud, 
		    _currentDepthImage.rows, 
		    _currentDepthImage.cols,
		    _currentCloudOffset,
		    _currentCameraMatrix, 
		    0.1);
      normalThumbnailBLOB->adjustFormat();
      depthThumbnailBLOB->adjustFormat();
      
      currentTrackerFrame->depthThumbnail.set(depthThumbnailBLOB);
      currentTrackerFrame->normalThumbnail.set(normalThumbnailBLOB);
      currentTrackerFrame->cameraMatrix = _currentCameraMatrix;
      currentTrackerFrame->imageRows = _currentDepthImage.rows;
      currentTrackerFrame->imageCols = _currentDepthImage.cols;
      Eigen::Isometry3d _iso;
      convertScalar(_iso, _globalT);
      currentTrackerFrame->setTransform(_iso);
      convertScalar(currentTrackerFrame->sensorOffset, _currentCloudOffset);
      currentTrackerFrame->setSeq(_seq++);
      _manager->addNode(currentTrackerFrame);
      //cerr << "AAAA" << endl;

      if (_cache)
	_currentCloudHandle=_cache->get(currentTrackerFrame);
      //cerr << "BBBB" << endl;
      //_cache->lock(currentTrackerFrame);
      newFrameCallbacks(currentTrackerFrame);
      currentTrackerFrame->depthThumbnail.set(0);
      currentTrackerFrame->normalThumbnail.set(0);
      currentTrackerFrame->depthImage.set(0);
    }

    if (newRelation) {
      PwnTrackerRelation* rel = new PwnTrackerRelation(_manager);
      rel->setTransform(relationMean);
      Matrix6d omega;
      convertScalar(omega, _aligner->omega());
      omega.setIdentity();
      omega *= 100;
      rel->setInformationMatrix(omega);
      rel->setTo(currentTrackerFrame);
      rel->setFrom(_previousTrackerFrame);
      //cerr << "saved relation" << _previousTrackerFrame << " " << currentTrackerFrame << endl;
      _manager->addRelation(rel);
      newRelationCallbacks(rel);
      //ser.writeObject(*rel);
    }

    if (currentTrackerFrame) {
      _previousTrackerFrame = currentTrackerFrame;
    }
  }
Beispiel #28
0
ZMPDistributor::ForceTorque ZMPDistributor::distributeZMP(const Eigen::Vector3f& localAnkleLeft,
                                                              const Eigen::Vector3f& localAnkleRight,
                                                              const Eigen::Matrix4f& leftFootPoseGroundFrame,
                                                              const Eigen::Matrix4f& rightFootPoseGroundFrame,
                                                              const Eigen::Vector3f& zmpRefGroundFrame,
                                                              Bipedal::SupportPhase phase)
{
    Eigen::Matrix4f groundPoseLeft  = Bipedal::projectPoseToGround(leftFootPoseGroundFrame);
    Eigen::Matrix4f groundPoseRight = Bipedal::projectPoseToGround(rightFootPoseGroundFrame);
    Eigen::Vector3f localZMPLeft    = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseLeft.inverse());
    Eigen::Vector3f localZMPRight   = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseRight.inverse());

    double alpha = computeAlpha(groundPoseLeft, groundPoseRight, zmpRefGroundFrame, localZMPLeft.head(2), localZMPRight.head(2), phase);

    //std::cout << "########## " << alpha << " ###########" << std::endl;

    ForceTorque ft;
    // kg*m/s^2 = N
    ft.leftForce  = -(1-alpha) * mass * gravity;
    ft.rightForce = -alpha     * mass * gravity;

    // Note we need force as kg*mm/s^2
    ft.leftTorque  = (localAnkleLeft  - localZMPLeft).cross(ft.leftForce * 1000);
    ft.rightTorque = (localAnkleRight - localZMPRight).cross(ft.rightForce * 1000);

    // ZMP not contained in convex polygone
    if (std::fabs(alpha) > std::numeric_limits<float>::epsilon()
     && std::fabs(alpha-1) > std::numeric_limits<float>::epsilon())
    {
        Eigen::Vector3f leftTorqueGroundFrame  = groundPoseLeft.block(0, 0, 3, 3)  * ft.leftTorque;
        Eigen::Vector3f rightTorqueGroundFrame = groundPoseRight.block(0, 0, 3, 3) * ft.rightTorque;
        Eigen::Vector3f tau0 = -1 * (leftTorqueGroundFrame + rightTorqueGroundFrame);

        //std::cout << "Tau0World: " << tau0.transpose() << std::endl;
        //std::cout << "leftTorqueWorld: "  << leftTorqueWorld.transpose() << std::endl;
        //std::cout << "rightTorqueWorld: " << rightTorqueWorld.transpose() << std::endl;

        // Note: Our coordinate system is different than in the paper!
        // Also this is not the same as the ground frame.
        Eigen::Vector3f xAxis = leftFootPoseGroundFrame.block(0,3,3,1) + localAnkleLeft
                              - localAnkleRight - rightFootPoseGroundFrame.block(0,3,3,1);
        xAxis /= xAxis.norm();
        Eigen::Vector3f zAxis(0, 0, 1);
        Eigen::Vector3f yAxis = zAxis.cross(xAxis);
        yAxis /= yAxis.norm();
        Eigen::Matrix3f centerFrame;
        centerFrame.block(0, 0, 3, 1) = xAxis;
        centerFrame.block(0, 1, 3, 1) = yAxis;
        centerFrame.block(0, 2, 3, 1) = zAxis;

        //std::cout << "Center frame:\n" << centerFrame << std::endl;

        Eigen::Vector3f centerTau0 = centerFrame.transpose() * tau0;
        Eigen::Vector3f leftTorqueCenter;
        leftTorqueCenter.x() = (1-alpha)*centerTau0.x();
        leftTorqueCenter.y() = centerTau0.y() < 0 ? centerTau0.y() : 0;
        leftTorqueCenter.z() = 0;
        Eigen::Vector3f rightTorqueCenter;
        rightTorqueCenter.x() = alpha*centerTau0.x();
        rightTorqueCenter.y() = centerTau0.y() < 0 ? 0 : centerTau0.y();
        rightTorqueCenter.z() = 0;

        //std::cout << "Tau0Center: " << centerTau0.transpose() << std::endl;
        //std::cout << "leftTorqueCenter: "  << leftTorqueCenter.transpose() << std::endl;
        //std::cout << "rightTorqueCenter: " << rightTorqueCenter.transpose() << std::endl;

        // tcp <--- ground frame <--- center frame
        ft.leftTorque  = groundPoseLeft.block(0, 0, 3, 3).transpose()  * centerFrame * leftTorqueCenter;
        ft.rightTorque = groundPoseRight.block(0, 0, 3, 3).transpose() * centerFrame * rightTorqueCenter;
    }

    // Torque depends on timestep, we need a way to do this correctly.
    const double torqueFactor = 1;
    // convert to Nm
    ft.leftTorque  *= torqueFactor / 1000.0 / 1000.0;
    ft.rightTorque *= torqueFactor / 1000.0 / 1000.0;

    //std::cout << "leftTorque: "  << ft.leftTorque.transpose() << std::endl;
    //std::cout << "rightTorque: " << ft.rightTorque.transpose() << std::endl;

    return ft;
}