示例#1
0
Eigen::Vector3d GetCameraCenterWorld() {
	GLdouble projmat[16];
	GLdouble modelmat[16];

	glGetDoublev( GL_PROJECTION_MATRIX, projmat);
	glGetDoublev( GL_MODELVIEW_MATRIX, modelmat);
	Eigen::Matrix4d InvModelMat;
	Eigen::Matrix4d InvProjMat;
	InvModelMat.setZero();
	InvProjMat.setZero();
	for(int i=0; i<4; i++){
		for(int j=0; j<4; j++){
			InvModelMat(i,j)=modelmat[4*i+j];
			InvProjMat(i,j)=projmat[4*i+j];
			//printf("%lf ", InvModelMat[i][j]);
		}
		//printf("\n");
	}
	InvModelMat=(InvModelMat*InvProjMat).transpose();
	InvModelMat = (InvModelMat.inverse());
	Eigen::Vector4d cc(0, 0, 0, 1);
	cc=InvModelMat*cc;
	if(cc.norm()!=0) cc=cc/cc[3];
	return Eigen::Vector3d(cc[0], cc[1], cc[2]);
}
void BezierBoundarySolver::_IterateCurvatureReduction(BezierBoundaryProblem* pProblem,Eigen::Vector4d& params)
{
    double epsilon = 0.0001;
    //create a jacobian for the parameters by perturbing them
    Eigen::Vector4d Jt; //transpose of the jacobian
    BezierBoundaryProblem origProblem = *pProblem;
    double maxK = _GetMaximumCurvature(pProblem);
    for(int ii = 0; ii < 4 ; ii++){
        Eigen::Vector4d epsilonParams = params;
        epsilonParams[ii] += epsilon;
        _Get5thOrderBezier(pProblem,epsilonParams);
        _Sample5thOrderBezier(pProblem);
        double kPlus = _GetMaximumCurvature(pProblem);

        epsilonParams[ii] -= 2*epsilon;
        _Get5thOrderBezier(pProblem,epsilonParams);
        _Sample5thOrderBezier(pProblem);
        double kMinus = _GetMaximumCurvature(pProblem);
        Jt[ii] = (kPlus-kMinus)/(2*epsilon);
    }

    //now that we have Jt, we can calculate JtJ
    Eigen::Matrix4d JtJ = Jt*Jt.transpose();
    //thikonov regularization
    JtJ += Eigen::Matrix4d::Identity();

    Eigen::Vector4d deltaParams = JtJ.inverse() * Jt*maxK;
    params -= deltaParams;
    _Get5thOrderBezier(pProblem,params);
    _Sample5thOrderBezier(pProblem);
    //double finalMaxK = _GetMaximumCurvature(pProblem);

    //dout("2D Iteration took k from " << maxK << " to " << finalMaxK);
}
void Dist_grad(const Eigen::Matrix<double,Eigen::Dynamic,1> &t,
			   const std::vector<skeleton::BranchContProjSkel::Ptr> &skel,
			   double &value,
			   Eigen::Matrix<double,Eigen::Dynamic,1> &gradient)
{
	Eigen::Matrix4d A = Eigen::Matrix4d::Zero();
	Eigen::Vector4d b = Eigen::Vector4d::Zero();
	value = 0.0;
	gradient = Eigen::Matrix<double,Eigen::Dynamic,1>::Zero(t.rows(),1);

	std::vector<Eigen::Vector4d> ori(skel.size());
	std::vector<Eigen::Vector4d> vec(skel.size());
	std::vector<Eigen::Vector4d> orijac(skel.size());
	std::vector<Eigen::Vector4d> vecjac(skel.size());
	std::vector<Eigen::Matrix4d> A_part_jac(skel.size());
	std::vector<Eigen::Vector4d> b_part_jac(skel.size());

	for(unsigned int i=0;i<skel.size();i++)
	{
		Compositor<Application<Eigen::Matrix<double,8,1>,Eigen::Vector3d>,Application<Eigen::Vector3d,double> >
			fun(skel[i]->getModel()->getR8Fun(),skel[i]->getCompFun());
		Eigen::Matrix<double,8,1> veclin = fun(t(i));
		Eigen::Matrix<double,8,1> jaclin = fun.jac(t(i));

		ori[i]  = veclin.block<4,1>(0,0);
		vec[i]  = veclin.block<4,1>(4,0).normalized();

		orijac[i] = jaclin.block<4,1>(0,0);
		vecjac[i] = jaclin.block<4,1>(4,0)*(1.0/veclin.block<4,1>(4,0).norm());

		Eigen::Matrix4d A_part = Eigen::Matrix4d::Identity() - vec[i]*vec[i].transpose();
		Eigen::Vector4d b_part = A_part*ori[i];

		A_part_jac[i] = - vecjac[i]*vec[i].transpose() - vec[i]*vecjac[i].transpose();
		b_part_jac[i] = A_part_jac[i] * ori[i] + A_part * orijac[i];

		A+=A_part;
		b+=b_part;
	}

	Eigen::Matrix4d A_inv = A.inverse();

	Eigen::Vector4d P = A_inv*b;
	for(unsigned int i=0;i<skel.size();i++)
	{
		Eigen::Vector4d P_jac = A_inv * A_part_jac[i] * P + A_inv * b_part_jac[i];

		double Dist = (P - ori[i]).squaredNorm() - pow((P - ori[i]).dot(vec[i]),2);

		gradient(i) =  2*(P_jac - orijac[i]).dot(P - ori[i])
					  -2*(P_jac - orijac[i]).dot(vec[i]) * (P - ori[i]).dot(vec[i])
					  -2*(P - ori[i]).dot(vecjac[i]) * (P - ori[i]).dot(vec[i]);

		value += Dist;
	}
}
	SMC_higher_order(/*systems::ExecutionManager* em*/bool status,
			const Eigen::Matrix4d coeff, const Eigen::Vector4d delta,
			const Eigen::Matrix4d A, const Eigen::Matrix4d B, const float P,
			const float Q,
			const std::string& sysName = "Slidingmode_Controller") :
			System(sysName), referencejpInput(this), referencejvInput(this), referencejaInput(
					this), feedbackjpInput(this), feedbackjvInput(this), M(
					this), C(this),

			controlOutput(this, &controlOutputValue), STATUS(status), Coeff(
					coeff), Delta(delta), A(A), B(B), P(P), Q(Q) {
		inv_B = B.inverse();
	}
示例#5
0
    double Keyframe::distance( const Eigen::Matrix4d & transform ) const
    {
        const Eigen::Matrix4d& poseMat = _pose.transformation();
        const Eigen::Matrix4d relativePose = poseMat * transform.inverse();

        double dist = relativePose.block<3, 1>( 0, 3 ).norm();

        //		const Eigen::Matrix3d & poseR = poseMat.block<3, 3>( 0, 0 );
        //		const Eigen::Matrix3d & kfR   = transform.block<3, 3>( 0, 0 );
        //		Eigen::Matrix3d deltaR = poseR.transpose() * kfR;
        //		Eigen::Vector3d euler = deltaR.eulerAngles( 0, 1, 2 );
        //		dist += euler.norm();

        return dist;
    }
示例#6
0
Eigen::Vector3d GetPointEyeToWorld(const Eigen::Ref<const Eigen::Vector3d>& _pt) {
	GLdouble modelmat[16];

	glGetDoublev( GL_MODELVIEW_MATRIX, modelmat);
	Eigen::Matrix4d InvModelMat;
	InvModelMat.setZero();
	for(int i=0; i<4; i++){
		for(int j=0; j<4; j++){
			InvModelMat(i,j)=modelmat[4*i+j];
		}
	}
	InvModelMat=(InvModelMat.transpose());
	InvModelMat=(InvModelMat.inverse());
	Eigen::Vector4d cc( _pt(0),_pt(1), _pt(2),1);
	cc=InvModelMat*cc;
	if(cc.norm()!=0) cc=cc/cc[3];
	return Eigen::Vector3d(cc[0], cc[1], cc[2]);
}
	virtual void operate() {
		tmp_theta_pos = this->feedbackjpInput.getValue();
		ThetaInput << tmp_theta_pos[0], tmp_theta_pos[1], tmp_theta_pos[2], tmp_theta_pos[3];
		M_tmp = this->M.getValue();
		Md_tmp = this->Md.getValue();
		tmp_theta_vel = this->feedbackjvInput.getValue();
		ThetadotInput << tmp_theta_vel[0], tmp_theta_vel[1], tmp_theta_vel[2], tmp_theta_vel[3];
		C_tmp = this->C.getValue();
		Cd_tmp = this->Cd.getValue();

		// Reference position, velocity and accelerations
		qd[0] = this->referencejpInput.getValue()[0];
		qd[1] = this->referencejpInput.getValue()[1];
		qd[2] = this->referencejpInput.getValue()[2];
		qd[3] = this->referencejpInput.getValue()[3];

		qdd[0] = this->referencejvInput.getValue()[0];
		qdd[1] = this->referencejvInput.getValue()[1];
		qdd[2] = this->referencejvInput.getValue()[2];
		qdd[3] = this->referencejvInput.getValue()[3];

		qddd[0] = this->referencejaInput.getValue()[0];
		qddd[1] = this->referencejaInput.getValue()[1];
		qddd[2] = this->referencejaInput.getValue()[2];
		qddd[3] = this->referencejaInput.getValue()[3];
//
		//Position error
		e = ThetaInput - qd;
		//Velocity error
		ed = ThetadotInput - qdd;
		//Eta
		eta = K / b;
		//The error matrix
		Xtilde.resize(8, 1);
		Xtilde << e, ed;

		Md_tmpinverse = Md_tmp.inverse();
		M_tmpinverse = M_tmp.inverse();


		F = -M_tmpinverse * (C_tmp);
		Fd = -Md_tmpinverse * (Cd_tmp);

		rho = F - Fd;

		// The fbar vector
		for (i = 0; i < 4; i = i + 1) {
			fbar[i] = rho[i] + eta[i] * c[i] * Xtilde[i]
					+ (c[i] + eta[i]) * Xtilde[4 + i] + eta[i] * phi[i];
		}

//

		Ftilde1 << Xtilde[4], -eta[0] * c[0] * Xtilde[0]
				- (c[0] + eta[0]) * Xtilde[4] - eta[0] * phi[0], 0;
		Ftilde2 << Xtilde[5], -eta[1] * c[1] * Xtilde[1]
				- (c[1] + eta[1]) * Xtilde[5] - eta[1] * phi[1], 0;
		Ftilde3 << Xtilde[6], -eta[2] * c[2] * Xtilde[2]
				- (c[2] + eta[2]) * Xtilde[6] - eta[2] * phi[2], 0;
		Ftilde4 << Xtilde[7], -eta[3] * c[3] * Xtilde[3]
				- (c[3] + eta[3]) * Xtilde[7] - eta[3] * phi[3], 0;

		//The L and Lbar vectors

		for (i = 0; i < 4; i = i + 1) {
			L[i] = 0.5
					* (Xtilde[i] * Xtilde[i] + Xtilde[4 + i] * Xtilde[4 + i]);
			Lbar[i] = L[i] + fbar[i] * fbar[i];

		}

		// The gradient matrices
		DVDX1
				<< (W1[0] * Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0])
						* W1[0], (W1[0] * Xtilde[0] + W1[1] * Xtilde[4]
				+ W1[2] * phi[0]) * W1[1], (W1[0] * Xtilde[0]
				+ W1[1] * Xtilde[4] + W1[2] * phi[0]) * W1[2];
		DVDX2
				<< (W2[0] * Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1])
						* W2[0], (W2[0] * Xtilde[1] + W2[1] * Xtilde[5]
				+ W2[2] * phi[1]) * W2[1], (W2[0] * Xtilde[1]
				+ W2[1] * Xtilde[5] + W2[2] * phi[1]) * W2[2];
		DVDX3
				<< (W3[0] * Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2])
						* W3[0], (W3[0] * Xtilde[2] + W3[1] * Xtilde[6]
				+ W3[2] * phi[2]) * W3[1], (W3[0] * Xtilde[2]
				+ W3[1] * Xtilde[6] + W3[2] * phi[2]) * W3[2];
		DVDX4
				<< (W4[0] * Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3])
						* W4[0], (W4[0] * Xtilde[3] + W4[1] * Xtilde[7]
				+ W4[2] * phi[3]) * W4[1], (W4[0] * Xtilde[3]
				+ W4[1] * Xtilde[7] + W4[2] * phi[3]) * W4[2];

		DVDW1
				<< (W1[0] * Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0])
						* Xtilde[0] + W1[0], (W1[0] * Xtilde[0]
				+ W1[1] * Xtilde[4] + W1[2] * phi[0]) * Xtilde[4] + W1[1], (W1[0]
				* Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0]) * phi[0]
				+ W1[2];
		DVDW2
				<< (W2[0] * Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1])
						* Xtilde[1] + W2[0], (W2[0] * Xtilde[1]
				+ W2[1] * Xtilde[5] + W2[2] * phi[1]) * Xtilde[5] + W2[1], (W2[0]
				* Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1]) * phi[1]
				+ W2[2];
		DVDW3
				<< (W3[0] * Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2])
						* Xtilde[2] + W3[0], (W3[0] * Xtilde[2]
				+ W3[1] * Xtilde[6] + W3[2] * phi[2]) * Xtilde[6] + W3[1], (W3[0]
				* Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2]) * phi[2]
				+ W3[2];
		DVDW4
				<< (W4[0] * Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3])
						* Xtilde[3] + W4[0], (W4[0] * Xtilde[3]
				+ W4[1] * Xtilde[7] + W4[2] * phi[3]) * Xtilde[7] + W4[1], (W4[0]
				* Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3]) * phi[3]
				+ W4[2];
////
		s1 = DVDW1.transpose();
		s2 = DVDW2.transpose();
		s3 = DVDW3.transpose();
		s4 = DVDW4.transpose();

		Eigen::Matrix3d tmp_Gtilde;
		tmp_Gtilde << Gtilde[0] * Gtilde[0], Gtilde[0] * Gtilde[1], Gtilde[0]
				* Gtilde[2], Gtilde[1] * Gtilde[0], Gtilde[1] * Gtilde[1], Gtilde[1]
				* Gtilde[2], Gtilde[2] * Gtilde[0], Gtilde[2] * Gtilde[1], Gtilde[2]
				* Gtilde[2];

//
		r1 = -Lbar[0] + 0.25 * DVDX1.dot(tmp_Gtilde * DVDX1)
				- DVDX1.dot(Ftilde1);
		r2 = -Lbar[1] + 0.25 * DVDX2.dot(tmp_Gtilde * DVDX2)
				- DVDX2.dot(Ftilde2);
		r3 = -Lbar[2] + 0.25 * DVDX3.dot(tmp_Gtilde * DVDX3)
				- DVDX3.dot(Ftilde3);
		r4 = -Lbar[3] + 0.25 * DVDX4.dot(tmp_Gtilde * DVDX4)
				- DVDX4.dot(Ftilde4);

		W1dot = s1.transpose() / (s1.dot(s1)) * r1;
		W2dot = s2.transpose() / (s2.dot(s2)) * r2;
		W3dot = s3.transpose() / (s3.dot(s3)) * r3;
		W4dot = s4.transpose() / (s4.dot(s4)) * r4;
////
		ueq1 = 0.5
				* (W1[0] * (W1[2] - W1[1]) * Xtilde[0]
						+ W1[1] * (W1[2] - W1[1]) * Xtilde[4]
						+ W1[2] * (W1[2] - W1[1]) * phi[0]);
		ueq2 = 0.5
				* (W2[0] * (W2[2] - W2[1]) * Xtilde[1]
						+ W2[1] * (W2[2] - W2[1]) * Xtilde[5]
						+ W2[2] * (W2[2] - W2[1]) * phi[1]);
		ueq3 = 0.5
				* (W3[0] * (W3[2] - W3[1]) * Xtilde[2]
						+ W3[1] * (W3[2] - W3[1]) * Xtilde[6]
						+ W3[2] * (W3[2] - W3[1]) * phi[2]);
		ueq4 = 0.5
				* (W4[0] * (W4[2] - W4[1]) * Xtilde[3]
						+ W4[1] * (W4[2] - W4[1]) * Xtilde[7]
						+ W4[2] * (W4[2] - W4[1]) * phi[3]);
//
// Final torque computations
		Ueq << ueq1, ueq2, ueq3, ueq4;

		Ud = qddd + Md_tmpinverse * Cd_tmp;

		Tau = M_tmp * (Ud + Ueq); // Final torque to system.
//

		phidot[0] = -fbar[0] - ueq1;
		phidot[1] = -fbar[1] - ueq2;
		phidot[2] = -fbar[2] - ueq3;
		phidot[3] = -fbar[3] - ueq4;

		phi = phi + phidot * del;
		W1 = W1 + W1dot * del;
		W2 = W2 + W2dot * del;
		W3 = W3 + W3dot * del;
		W4 = W4 + W4dot * del;

		torque_tmp[0] = Tau[0];
		torque_tmp[1] = Tau[1];
		torque_tmp[2] = Tau[2];
		torque_tmp[3] = Tau[3];

//		torque_tmp[0] = 0;
//		torque_tmp[1] = 0;
//		torque_tmp[2] = 0;
//		torque_tmp[3] = 0;

		optslidecontrolOutputValue->setData(&torque_tmp);



	}
示例#8
0
bool SurfelsRGBDModel::addNewView(const PCloud &cloud_d, Eigen::Matrix4d T_world){
	
	
	pcl::KdTreeFLANN< pcl::PointXYZRGB > kdtree;
	kdtree.setInputCloud (cloud_d);
	std::vector<int> indice(1);
	std::vector<float> distance(1);
	
	std::vector<int> covered_pixels(cloud_d->size(),0);
	
	Eigen::Vector4d auxp;
	Eigen::Vector4d surfel_normal;
	Eigen::Vector4d cloudd_normal;
	
	pcl::PointXYZRGB surfel_pt;
	
	int ptoremove=0;
	pcl::PointSurfel remove_aux;
	
	
	//update surfels
	for(unsigned int i=0; i<m_surfels.size()-ptoremove; i++){
			
		auxp << m_surfels[i].x, m_surfels[i].y, m_surfels[i].z, 1.0;
		//std::cout << auxp.transpose() <<  "\n";
		auxp = T_world.inverse()*auxp;
		
		surfel_pt.x = auxp(0);
		surfel_pt.y = auxp(1);
		surfel_pt.z = auxp(2);
	
		//std::cout << surfel_pt.x  << " "  << surfel_pt.y  << " " << surfel_pt.z <<  "\n\n";
		
		//checks if there is any point in range
		if (kdtree.nearestKSearch (surfel_pt, 1, indice, distance) != 1) continue;
		
		//std::cout << "depois tree " << distance[0] << " " << mindist*mindist << " " << mindist <<"\n";
		
		if (distance[0]>mindist*mindist){

			if (m_surfels[i].confidence < 2)
			{	
				remove_aux=m_surfels[i];
				m_surfels[i]=m_surfels[m_surfels.size()-1-ptoremove];
				m_surfels[m_surfels.size()-1-ptoremove]=remove_aux;
				ptoremove++;
				i--;	
			}
			continue;
		}
		
		//checks normal angles
		surfel_normal << m_surfels[i].normal_x,m_surfels[i].normal_y, m_surfels[i].normal_z, 1.0;
		cloudd_normal = computeNormal(indice[0],&kdtree,cloud_d);
		surfel_normal =T_world.inverse()*surfel_normal;
		normalize(surfel_normal);

		
		float normal_angle = acos(cloudd_normal(0)*surfel_normal(0) + cloudd_normal(1)*surfel_normal(1) + cloudd_normal(2)*surfel_normal(2));
		
		//std::cout << normal_angle*180.0/3.14159265 <<  " " << update_max_normal_angle << "\n";
		
		if (normal_angle > (update_max_normal_angle*3.14159265/180.0))
		{
			// Removal check. If a surfel has a different normal and is closer to the camera
			// than the new scan, remove it.
			if ((surfel_pt.z > cloud_d->points[indice[0]].z) && (m_surfels[i].confidence < 2))
			{	
				remove_aux=m_surfels[i];
				m_surfels[i]=m_surfels[m_surfels.size()-1-ptoremove];
				m_surfels[m_surfels.size()-1-ptoremove]=remove_aux;
				ptoremove++;
				i--;	
			}else
				covered_pixels[indice[0]] = 1;
			continue;
		}
		
		
		//checks distance
		// If existing surfel is far from new depth value:
		// - If existing one had a worst point of view, and was seen only once, remove it.
		// - Otherwise do not include the new one.
		if (std::fabs(surfel_pt.z - cloud_d->points[indice[0]].z) > update_max_dist){
			
			if (m_surfels[i].confidence < 2){
			  remove_aux=m_surfels[i];
			  m_surfels[i]=m_surfels[m_surfels.size()-1-ptoremove];
			  m_surfels[m_surfels.size()-1-ptoremove]=remove_aux;
			  ptoremove++;
			  i--;
			}
			else
			  covered_pixels[indice[0]] = 1;
			continue;
		}
		
		
		
		
		//update surfel
		m_surfels[i].x = (m_surfels[i].x + cloud_d->points[indice[0]].x*T_world(0,0) + cloud_d->points[indice[0]].y*T_world(0,1) + cloud_d->points[indice[0]].z*T_world(0,2) + T_world(0,3))/2;
		m_surfels[i].y = (m_surfels[i].y + cloud_d->points[indice[0]].x*T_world(1,0) + cloud_d->points[indice[0]].y*T_world(1,1) + cloud_d->points[indice[0]].z*T_world(1,2) + T_world(1,3))/2;
		m_surfels[i].z = (m_surfels[i].z + cloud_d->points[indice[0]].x*T_world(2,0) + cloud_d->points[indice[0]].y*T_world(2,1) + cloud_d->points[indice[0]].z*T_world(2,2) + T_world(2,3))/2;
	
		Eigen::Vector4d cloudd_normal2 = T_world*cloudd_normal;
		normalize(cloudd_normal2);
		
		m_surfels[i].normal_x = (m_surfels[i].normal_x + cloudd_normal2(0))/2;
		m_surfels[i].normal_y = (m_surfels[i].normal_y + cloudd_normal2(1))/2;
		m_surfels[i].normal_z = (m_surfels[i].normal_z + cloudd_normal2(2))/2;
	
		unsigned char rgba_ptr[3]; 
		rgba_ptr[0] = m_surfels[i].rgba >> 24;		
		rgba_ptr[1] = m_surfels[i].rgba >> 16; 		
		rgba_ptr[2] = m_surfels[i].rgba >> 8; 		
		rgba_ptr[0] = (int)( (int)rgba_ptr[0] + cloud_d->points[indice[0]].r )/2; 
		rgba_ptr[1] = (int)( (int)rgba_ptr[1] + cloud_d->points[indice[0]].g )/2;
		rgba_ptr[2] = (int)( (int)rgba_ptr[2] + cloud_d->points[indice[0]].b )/2;
		
		m_surfels[i].rgba = (rgba_ptr[0] <<  24) | (rgba_ptr[1] << 16) | (rgba_ptr[2] << 8);
		 
		m_surfels[i].radius = std::min((double)m_surfels[i].radius, (1.0/1.414213562) * cloud_d->points[indice[0]].z / (fx * cloudd_normal(2)));
		m_surfels[i].confidence = m_surfels[i].confidence + 1.0;
		covered_pixels[indice[0]] = 1;
	}
	
	//Add new surfels
	// Surfel addition
    for (unsigned int i = 0; i < cloud_d->size(); i++) 
    {
      if (isnan(cloud_d->points[i].z) || covered_pixels[i]==1) continue;
      
		pcl::PointSurfel surfel;
      
		surfel.x = cloud_d->points[i].x*T_world(0,0) + cloud_d->points[i].y*T_world(0,1) + cloud_d->points[i].z*T_world(0,2) + T_world(0,3);
		surfel.y = cloud_d->points[i].x*T_world(1,0) + cloud_d->points[i].y*T_world(1,1) + cloud_d->points[i].z*T_world(1,2) + T_world(1,3);
		surfel.z = cloud_d->points[i].x*T_world(2,0) + cloud_d->points[i].y*T_world(2,1) + cloud_d->points[i].z*T_world(2,2) + T_world(2,3);
	
		cloudd_normal = computeNormal(i,&kdtree,cloud_d);
		cloudd_normal = T_world*cloudd_normal;
		normalize(cloudd_normal);
		surfel.normal_x = cloudd_normal(0);
		surfel.normal_y = cloudd_normal(1);
		surfel.normal_z = cloudd_normal(2);
		 
		surfel.rgba = (cloud_d->points[i].r <<  24) | (cloud_d->points[i].g << 16) | (cloud_d->points[i].b << 8);
		 
		double camera_normal_z = std::max((float)cloudd_normal(2), 0.3f);
		surfel.radius = (2.0/1.414213562) * cloud_d->points[i].z / (fx * camera_normal_z);
		
		surfel.confidence = 1.0;
      
		m_surfels.push_back(surfel);
	}
	
	//remove Surfels
	for(int k=0;k<ptoremove;k++){
		m_surfels.pop_back();
	}
  	
}
示例#9
0
void GlobalOptimization::optimize()
{
    while(true)
    {
        if(newGPS)
        {
            newGPS = false;
            printf("global optimization\n");
            TicToc globalOptimizationTime;

            ceres::Problem problem;
            ceres::Solver::Options options;
            options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
            //options.minimizer_progress_to_stdout = true;
            //options.max_solver_time_in_seconds = SOLVER_TIME * 3;
            options.max_num_iterations = 5;
            ceres::Solver::Summary summary;
            ceres::LossFunction *loss_function;
            loss_function = new ceres::HuberLoss(1.0);
            ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();

            //add param
            mPoseMap.lock();
            int length = localPoseMap.size();
            // w^t_i   w^q_i
            double t_array[length][3];
            double q_array[length][4];
            map<double, vector<double>>::iterator iter;
            iter = globalPoseMap.begin();
            for (int i = 0; i < length; i++, iter++)
            {
                t_array[i][0] = iter->second[0];
                t_array[i][1] = iter->second[1];
                t_array[i][2] = iter->second[2];
                q_array[i][0] = iter->second[3];
                q_array[i][1] = iter->second[4];
                q_array[i][2] = iter->second[5];
                q_array[i][3] = iter->second[6];
                problem.AddParameterBlock(q_array[i], 4, local_parameterization);
                problem.AddParameterBlock(t_array[i], 3);
            }

            map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;
            int i = 0;
            for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++)
            {
                //vio factor
                iterVIONext = iterVIO;
                iterVIONext++;
                if(iterVIONext != localPoseMap.end())
                {
                    Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity();
                    Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity();
                    wTi.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIO->second[3], iterVIO->second[4], 
                                                               iterVIO->second[5], iterVIO->second[6]).toRotationMatrix();
                    wTi.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIO->second[0], iterVIO->second[1], iterVIO->second[2]);
                    wTj.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIONext->second[3], iterVIONext->second[4], 
                                                               iterVIONext->second[5], iterVIONext->second[6]).toRotationMatrix();
                    wTj.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIONext->second[0], iterVIONext->second[1], iterVIONext->second[2]);
                    Eigen::Matrix4d iTj = wTi.inverse() * wTj;
                    Eigen::Quaterniond iQj;
                    iQj = iTj.block<3, 3>(0, 0);
                    Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3);

                    ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),
                                                                                iQj.w(), iQj.x(), iQj.y(), iQj.z(),
                                                                                0.1, 0.01);
                    problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i+1], t_array[i+1]);
                }
                //gps factor
                double t = iterVIO->first;
                iterGPS = GPSPositionMap.find(t);
                if (iterGPS != GPSPositionMap.end())
                {
                    ceres::CostFunction* gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1], 
                                                                       iterGPS->second[2], iterGPS->second[3]);
                    //printf("inverse weight %f \n", iterGPS->second[3]);
                    problem.AddResidualBlock(gps_function, loss_function, t_array[i]);

                }

            }
            //mPoseMap.unlock();
            ceres::Solve(options, &problem, &summary);
            //std::cout << summary.BriefReport() << "\n";

            // update global pose
            //mPoseMap.lock();
            iter = globalPoseMap.begin();
            for (int i = 0; i < length; i++, iter++)
            {
            	vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2],
            							  q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};
            	iter->second = globalPose;
            	if(i == length - 1)
            	{
            	    Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity(); 
            	    Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();
            	    double t = iter->first;
            	    WVIO_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(localPoseMap[t][3], localPoseMap[t][4], 
            	                                                       localPoseMap[t][5], localPoseMap[t][6]).toRotationMatrix();
            	    WVIO_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(localPoseMap[t][0], localPoseMap[t][1], localPoseMap[t][2]);
            	    WGPS_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(globalPose[3], globalPose[4], 
            	                                                        globalPose[5], globalPose[6]).toRotationMatrix();
            	    WGPS_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(globalPose[0], globalPose[1], globalPose[2]);
            	    WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();
            	}
            }
            updateGlobalPath();
            //printf("global time %f \n", globalOptimizationTime.toc());
            mPoseMap.unlock();
        }
        std::chrono::milliseconds dura(2000);
        std::this_thread::sleep_for(dura);
    }
	return;
}
示例#10
0
  static bool colorize(const drc::map_image_t& iDepthMap,
                       const Eigen::Affine3d& iLocalToCamera,
                       const bot_core::image_t& iImage,
                       const BotCamTrans* iCamTrans,
                       bot_core::image_t& oImage) {
    oImage.utime = iDepthMap.utime;
    oImage.width = iDepthMap.width;
    oImage.height = iDepthMap.height;
    oImage.row_stride = 3*iDepthMap.width;
    oImage.pixelformat = PIXEL_FORMAT_RGB;
    oImage.size = oImage.row_stride * oImage.height;
    oImage.nmetadata = 0;
    oImage.data.resize(oImage.size);

    Eigen::Matrix4d xform;
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 4; ++j) {
        xform(i,j) = iDepthMap.transform[i][j];
      }
    }
    xform = iLocalToCamera.matrix()*xform.inverse();

    for (int i = 0; i < iDepthMap.height; ++i) {
      for (int j = 0; j < iDepthMap.width; ++j) {
        double z;
        if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_FLOAT32) {
          z = ((float*)(&iDepthMap.data[0] + i*iDepthMap.row_bytes))[j];
          if (z < -1e10) {
            continue;
          }
        }
        else if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_UINT8) {
          uint8_t val = iDepthMap.data[i*iDepthMap.row_bytes + j];
          if (val == 0) {
            continue;
          }
          z = val;
        }
        else {
          continue;
        }

        Eigen::Vector4d pt = xform*Eigen::Vector4d(j,i,z,1);
        double p[3] = {pt(0)/pt(3),pt(1)/pt(3),pt(2)/pt(3)};
        double pix[3];
        bot_camtrans_project_point(iCamTrans, p, pix);
        if (pix[2] < 0) {
          continue;
        }
        uint8_t r,g,b;
        if (!interpolate(pix[0], pix[1], iImage, r, g, b)) {
          continue;
        }
        int outImageIndex = i*oImage.row_stride + 3*j;
        oImage.data[outImageIndex+0] = r;
        oImage.data[outImageIndex+1] = g;
        oImage.data[outImageIndex+2] = b;
      }
    }

    return true;
  }
示例#11
0
  static bool colorize(const drc::map_image_t& iDepthMap,
                       const bot_core::image_t& iImage,
                       typename pcl::PointCloud<T>::Ptr& oCloud) {
    int w(iDepthMap.width), h(iDepthMap.height);
    if ((w != iImage.width) || (h != iImage.height)) {
      return false;
    }

    // TODO: for now, reject compressed depth maps
    if (iDepthMap.compression != drc::map_image_t::COMPRESSION_NONE) {
      return false;
    }

    // TODO: for now, only accept rgb3 images
    if (iImage.pixelformat != PIXEL_FORMAT_RGB) {
      return false;
    }

    if (oCloud == NULL) {
      oCloud.reset(new pcl::PointCloud<T>());
    }
    oCloud->points.clear();

    Eigen::Matrix4d xform;
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 4; ++j) {
        xform(i,j) = iDepthMap.transform[i][j];
      }
    }
    xform = xform.inverse();

    for (int i = 0; i < h; ++i) {
      for (int j = 0; j < w; ++j) {
        double z;
        if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_FLOAT32) {
          z = ((float*)(&iDepthMap.data[0] + i*iDepthMap.row_bytes))[j];
          if (z < -1e10) {
            continue;
          }
        }
        else if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_UINT8) {
          uint8_t val = iDepthMap.data[i*iDepthMap.row_bytes + j];
          if (val == 0) {
            continue;
          }
          z = val;
        }
        else {
          continue;
        }

        Eigen::Vector4d pt = xform*Eigen::Vector4d(j,i,z,1);
        T newPoint;
        newPoint.x = pt(0)/pt(3);
        newPoint.y = pt(1)/pt(3);
        newPoint.z = pt(2)/pt(3);
        int imageIndex = i*iImage.row_stride + 3*j;
        newPoint.r = iImage.data[imageIndex+0];
        newPoint.g = iImage.data[imageIndex+1];
        newPoint.b = iImage.data[imageIndex+2];
        oCloud->points.push_back(newPoint);
      }
    }
    oCloud->width = oCloud->points.size();
    oCloud->height = 1;
    oCloud->is_dense = false;

    return true;
  }
示例#12
0
	/*
	 * here comes the object cluster point cloud in camera frame of reference
	 */
	void dataCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud) {

		if( !create_map_ )
			return;

		ROS_INFO("creating map");
		object_mutex.lock();
		ROS_INFO("dataCallback got the mutex");


		pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudIn = pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new pcl::PointCloud<pcl::PointXYZRGB>() );
		pcl::fromROSMsg(*point_cloud, *pointCloudIn);

//		Eigen::Matrix4d objectTransform = Eigen::Matrix4d::Identity();

		// change reference frame of point cloud to point mean and oriented along principal axes
		Eigen::Vector4d mean;
		Eigen::Vector3d eigenvalues;
		Eigen::Matrix3d cov;
		Eigen::Matrix3d eigenvectors;
		pcl::computeMeanAndCovarianceMatrix( *pointCloudIn, cov, mean );
		pcl::eigen33( cov, eigenvectors, eigenvalues );





		/*
		 * Added comment to this misterious sign change:
		 * Assuming the eigenvectors of the object come in the camera frame of reference
		 * this means the unit vector along the Z axis (Eigen::Vector3d::UnitZ()) points away from the camera.
		 * The LAST eigenvector points upwards/downwards. If their dot product is positive
		 * means the eigenvector points downwards. This sign change would
		 * normalize it so that it always points downwards
		 *
		 */
		// z eigenvector
		if( Eigen::Vector3d(eigenvectors.col(0)).dot( Eigen::Vector3d::UnitZ() ) > 0.0 )
			eigenvectors.col(0) = (-eigenvectors.col(0)).eval();


		Eigen::Matrix4d objectTransform = Eigen::Matrix4d::Identity();
		objectTransform.block<3,1>(0,0) = eigenvectors.col(2);
		objectTransform.block<3,1>(0,1) = eigenvectors.col(1);
		objectTransform.block<3,1>(0,2) = eigenvectors.col(0);
		objectTransform.block<3,1>(0,3) = mean.block<3,1>(0,0);



		if( objectTransform.block<3,3>(0,0).determinant() < 0 ) {
			// x eigenvector (z axis)
			objectTransform.block<3,1>(0,0) = -objectTransform.block<3,1>(0,0);
		}

		/*
		 * -----------------------------
		 * set roll and pitch to 0,
		 * leave only the yaw as a free parameter
		 */
		//flatten_roll_pitch(objectTransform);
		/*
		 * ---------------------------
		 */


		eigenvectors.col(2) = objectTransform.block<3,1>(0,0); // x axis
		eigenvectors.col(1) = objectTransform.block<3,1>(0,1); // y axis
		eigenvectors.col(0) = objectTransform.block<3,1>(0,2); // z axis
		if(normalise_transform(eigenvectors,point_cloud)){
			// x eigenvector
			objectTransform.block<3,1>(0,0) = -objectTransform.block<3,1>(0,0);
			// y eigenvector
			objectTransform.block<3,1>(0,1) = -objectTransform.block<3,1>(0,1);
		}



		// transform from camera frame of reference to object's main axes frame of reference
		Eigen::Matrix4d objectTransformInv = objectTransform.inverse();

		//the MRSmap of the object is stored in the frame of reference of the objects' eigen axis
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr objectPointCloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new pcl::PointCloud<pcl::PointXYZRGB>() );
		pcl::transformPointCloud( *pointCloudIn, *objectPointCloud, (objectTransformInv).cast<float>() );

		objectPointCloud->sensor_origin_ = objectTransformInv.block<4,1>(0,3).cast<float>();
		objectPointCloud->sensor_orientation_ = Eigen::Quaternionf( objectTransformInv.block<3,3>(0,0).cast<float>() );
		
		treeNodeAllocator_->reset();
		map_ = boost::shared_ptr< MultiResolutionSurfelMap >( new MultiResolutionSurfelMap( max_resolution_, max_radius_, treeNodeAllocator_ ) );

		map_->params_.dist_dependency = dist_dep_;

		std::vector< int > pointIndices( objectPointCloud->points.size() );
		for( unsigned int i = 0; i < pointIndices.size(); i++ ) pointIndices[i] = i;
		map_->imageAllocator_ = imageAllocator_;
		map_->addPoints( *objectPointCloud, pointIndices );
		map_->octree_->root_->establishNeighbors();
		map_->evaluateSurfels();
		map_->buildShapeTextureFeatures();

		map_->save( map_folder_ + "/" + object_name_ + ".map" );

		if( init_frame_ != "" ) {

			ROS_INFO_STREAM( "Looking up transform to <init_frame>=" << init_frame_ <<" from <point_cloud->header.frame_id>="<<point_cloud->header.frame_id );

			try {

				tf::StampedTransform tf;
				tf_listener_->lookupTransform( init_frame_, point_cloud->header.frame_id, point_cloud->header.stamp, tf );
				Eigen::Affine3d init_frame_transform;
				tf::transformTFToEigen( tf, init_frame_transform );
				//obtains the transform of the object frame to the base_link frame of reference
				objectTransform = (init_frame_transform.matrix() * objectTransform).eval();


			}
				catch (tf::TransformException ex){
				ROS_ERROR("%s",ex.what());
			}


		}

		{

			Eigen::Quaterniond q( objectTransform.block<3,3>(0,0) );

			std::ofstream initPoseFile( map_folder_ + "/" + object_name_ + ".pose" );
			initPoseFile << "# x y z qx qy qz qw" << std::endl;
			initPoseFile << objectTransform(0,3) << " " << objectTransform(1,3) << " " << objectTransform(2,3) << " "
					 << q.x() << " " << q.y() << " " << q.z() << " "  << q.w() << std::endl;
			initPoseFile << "# init_pose: { position: { x: " << objectTransform(0,3) << ", y: " << objectTransform(1,3) << ", z: " << objectTransform(2,3)
					<< " }, orientation: { x: " << q.x() << ", y: " << q.y() << ", z: " << q.z() << ", w: " << q.w() << " } } }" << std::endl;
		}

		cloudv = pcl::PointCloud< pcl::PointXYZRGB >::Ptr( new pcl::PointCloud< pcl::PointXYZRGB >() );
		cloudv->header.frame_id = object_name_;
		map_->visualize3DColorDistribution( cloudv, -1, -1, false );


		Eigen::Quaterniond q( objectTransform.block<3,3>(0,0) );

		object_tf_.setIdentity();
		object_tf_.setRotation( tf::Quaternion( q.x(), q.y(), q.z(), q.w() ) );
		object_tf_.setOrigin( tf::Vector3( objectTransform(0,3), objectTransform(1,3), objectTransform(2,3) ) );

		//the object tf is from the camera frame of reference to the object's
		object_tf_.stamp_ = point_cloud->header.stamp;
		object_tf_.child_frame_id_ = object_name_;
		object_available = true; //the object_tf_ member variable now matches the last requested snapshot

		if( init_frame_ == "" ) {
			object_tf_.frame_id_ = point_cloud->header.frame_id;
		}
		else
			object_tf_.frame_id_ = init_frame_;

		first_publication_time = ros::Time::now();
		ROS_INFO_STREAM("first_publication_time="<<first_publication_time<<" do_publish_tf_="<<do_publish_tf_);
		if(do_publish_tf_){
			//ROS_INFO_STREAM("object_tf_.frame_id_="<<object_tf_.frame_id_);
			ROS_INFO_STREAM("> Snapshot map :dataCallback() publishing transform object_tf_.frame_id_ (init_frame_)=" << object_tf_.frame_id_ << " child="<< object_tf_.child_frame_id_<<std::endl);
//			std::cout <<"Press enter to continue..."<<std::endl;
//			std::cin.get();
			tf_broadcaster.sendTransform( object_tf_ );

		}

		sub_cloud_.shutdown();

		create_map_ = false;

		responseId_++;
		object_available = true;
		object_mutex.unlock();

		ROS_INFO("dataCallback released the mutex");

	}