vector<float> controller::getTargetPosition(int leg_id) {
    //Last link
    //translation
    Eigen::MatrixXf mt = Eigen::MatrixXf::Identity(4, 4);
    mt(2, 3) = body_bag->getFootLinkLength(leg_id, 3);
    //rotation
    Eigen::MatrixXf mr = Eigen::MatrixXf::Identity(4, 4);
    const dReal *rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 4; j++) {
            mr(i, j) = rotation_matrix_ode[i*4+j];
        }
    }
    mr(3, 0) = 0;
    mr(3, 1) = 0;
    mr(3, 2) = 0;
    mr(3, 3) = 1;

    Eigen::MatrixXf point = Eigen::MatrixXf::Random(4, 1);
    point(0, 0) = current_foot_location[leg_id][0];
    point(1, 0) = current_foot_location[leg_id][1];
    point(2, 0) = current_foot_location[leg_id][2];
    point(3, 0) = 1;
    Eigen::MatrixXf transformedPoint = mr*mt*mr.inverse()*point;

    //Second last link
    //translation
    mt = Eigen::MatrixXf::Identity(4, 4);
    mt(2, 3) = body_bag->getFootLinkLength(leg_id, 2);
    //rotation
    mr = Eigen::MatrixXf::Identity(4, 4);
    rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 2));
    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 4; j++) {
            mr(i, j) = rotation_matrix_ode[i*4+j];
        }
    }
    mr(3, 0) = 0;
    mr(3, 1) = 0;
    mr(3, 2) = 0;
    mr(3, 3) = 1;

    Eigen::MatrixXf endEffectorM = mr*mt*mr.inverse()*transformedPoint;

    vector<float> endEffector(3);
    endEffector[0] = endEffectorM(0,0);
    endEffector[1] = endEffectorM(1,0);
    endEffector[2] = endEffectorM(2,0);
    return endEffector;
}
			double weightedScaling(const Eigen::VectorXf& virtualInput,
					Matrix* scaled, Vector* taui)
			{
				Eigen::VectorXf tauIdeal = BinvIdeal*virtualInput;
				Eigen::VectorXf tmax(tauIdeal.size());

				for (int i=0; i<tmax.size(); ++i)
				{
					tmax(i) = ((tauIdeal(i)>=0)?posDir[i]:fabs(negDir[i]));
				}

				tmax=(tmax/(tmax.minCoeff())).cwiseInverse();

				Eigen::MatrixXf W = tmax.asDiagonal();
				Eigen::MatrixXf Winv = W.inverse();
				Eigen::MatrixXf Binv = Winv*B.transpose()*(B*Winv*B.transpose()).inverse();
				std::cout<<Binv<<std::endl;
				Eigen::VectorXf tdes = Binv*virtualInput;

				double scale_max = 1;
				for (size_t i=0; i<tdes.rows();++i)
				{
					double scale = fabs((tdes(i)>0)?tdes(i)/posDirC[i]:tdes(i)/negDirC[i]);
					if (scale>scale_max) scale_max=scale;
				}
				tdes = tdes/scale_max;
				(*taui) = tdes;
				(*scaled) = B*tdes;

				std::cout<<"Input:"<<virtualInput<<std::endl;
				std::cout<<"Output:"<<*scaled<<std::endl;


				return scale_max;
			}
Esempio n. 3
0
void
Chain::solveJacobian(const float stepSize, 
                     const int maxIterations,
                     const vec2& desiredPos, 
                     std::vector<float>* iterPose)
{
    vec3 G = vec3(desiredPos, 1); 
    vec3 endEffector; worldEndPos(endEffector);
   
    Eigen::Vector2f error;
    error(0) = G.x - endEffector.x;
    error(1) = G.y - endEffector.y;
    
    int iter = 0;
    int linkCount = count();
    while (iter < maxIterations  && error.norm() > EPSILON) { 
        Eigen::Vector2f dx = error * stepSize;
        
        // Calculate jacobian
        Eigen::MatrixXf jacobian(2,linkCount); 
        for (int j = 0; j < linkCount; j++) {
            vec3 jo; worldJointPos(j, jo);
            vec3 w = endEffector - jo;
            vec3 temp = cross(vec3(0,0,1.0f),vec3(w.x, w.y, 1));
            jacobian(0,j) = temp.x;
            jacobian(1,j) = temp.y;
        }
        Eigen::MatrixXf jacobianT = jacobian.transpose();
        Eigen::MatrixXf invJacobian = jacobian * jacobianT;
        invJacobian = jacobianT * invJacobian.inverse(); 
        Eigen::Vector4f delta = invJacobian * dx;
        for (int i = 0; i < linkCount; i++) {
            Link * l = getLink(i);
            if (delta(i) != delta(i)) {
                // Nudge angles arbitrarily to
                // break singularity for next iteration
                l->angle += 1;
            } else {
                l->angle += delta(i); 
            }
            if (iterPose) {
                iterPose->push_back(l->angle);
            }
        }
        
        // Update world frames for all joints and end effector
        worldEndPos(endEffector);    

        error(0) = G.x - endEffector.x;
        error(1) = G.y - endEffector.y;
        iter++;
    }
}
Esempio n. 4
0
//params is a matrix of nx2 where n is the number of landmarks
//for each landmark, the x and y pose of where it is
//pose is a matrix of 2x1 containing the initial guess of the robot pose
//delta is a matrix of 2x1 returns the increment in the x and y of the robot
void LMAlgr::computeIncrement(Eigen::MatrixXf params, Eigen::MatrixXf pose, double lambda, Eigen::MatrixXf error, Eigen::MatrixXf &delta){
	Eigen::MatrixXf Jac;
	Jac.resize(params.rows(), 2);
	//initialize the jacobian matrix
	for(int i = 0; i < params.rows(); i++){
		Jac(i, 0) = (params(i, 1) - pose(1, 0)) / (pow(params(i, 0) - pose(0, 0), 2) + pow(params(i, 1) - pose(1, 0), 2));
		Jac(i, 1) = -1 * (params(i, 0) - pose(0, 0)) / (pow(params(i, 0) - pose(0, 0), 2) + pow(params(i, 1) - pose(1, 0), 2));
	}
	Eigen::MatrixXf I;
	I = Eigen::MatrixXf::Identity(2, 2);
	Eigen::MatrixXf tmp = (Jac.transpose() * Jac) + (lambda * I);
	Eigen::MatrixXf incr = tmp.inverse() * Jac.transpose() * error;
	delta = incr;
}
Esempio n. 5
0
Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf &m) const
{
#ifdef CHECK_PERFORMANCE
	clock_t startT = clock();
#endif
	MatrixXf pseudo;
	switch (inverseMethod)
	{
	case eTranspose:
		{
			if (jointWeights.rows() == m.cols())
			{
				Eigen::MatrixXf W = jointWeights.asDiagonal();
				Eigen::MatrixXf W_1 = W.inverse();
				pseudo = W_1 * m.transpose() * (m*W_1*m.transpose()).inverse();
			}
			else
			{
				pseudo = m.transpose() * (m*m.transpose()).inverse();
			}
			break;
		}
	case eSVD:
		{
				 float pinvtoler = 0.00001f;
				 pseudo = MathTools::getPseudoInverse(m, pinvtoler);
				 break;
		}
	case eSVDDamped:
		{
				 float pinvtoler = 0.00001f;
				 pseudo = MathTools::getPseudoInverseDamped(m,pinvtoler);
				 break;
		}
	default:
		THROW_VR_EXCEPTION("Inverse Jacobi Method nyi...");
	}
#ifdef CHECK_PERFORMANCE
	clock_t endT = clock();
	float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
	//if (diffClock>10.0f)
	cout << "Inverse Jacobi time:" << diffClock << endl;
#endif
	return pseudo;
}
Esempio n. 6
0
void FeatureEval::computeCorrelation(float * data, int vector_size, int size, std::vector<int> & big_to_small, int stride, int offset){

    stride = stride ? stride : vector_size;

    if(ll_->getSelection().size() == 0)
        return;

    std::set<uint16_t> labels;
    for(boost::weak_ptr<Layer> wlayer: ll_->getSelection()){
        for(uint16_t label : wlayer.lock()->getLabelSet())
            labels.insert(label);
    }

    std::vector<float> layer = get_scaled_layer_mask(cl_->active_->labels_,
                          labels,
                          big_to_small,
                          size);

    Eigen::MatrixXf correlation_mat = multi_correlate(layer, data, vector_size, size, stride, offset);
    Eigen::MatrixXf Rxx = correlation_mat.topLeftCorner(vector_size, vector_size);
    Eigen::VectorXf c = correlation_mat.block(0, vector_size, vector_size, 1);

    //std::cout << correlation_mat << std::endl;

    float R = c.transpose() * (Rxx.inverse() * c);

    qDebug() << "R^2: " << R;
    //qDebug() << "R: " << sqrt(R);

    reportResult(R, c.data(), vector_size);

    //Eigen::VectorXf tmp = (Rxx.inverse() * c);

    qDebug() << "Y -> X correlation <<<<<<<<<<<<<";
    std::cout << c << std::endl;
    //qDebug() << "Coefs <<<<<<<<<<<<<";
    //std::cout << tmp << std::endl;

}
bool Mapper::getBoundedMap(ethzasl_icp_mapper::GetBoundedMap::Request &req, ethzasl_icp_mapper::GetBoundedMap::Response &res)
{
	if (!mapPointCloud)
		return false;

	const float max_x = req.topRightCorner.x;
	const float max_y = req.topRightCorner.y;
	const float max_z = req.topRightCorner.z;

	const float min_x = req.bottomLeftCorner.x;
	const float min_y = req.bottomLeftCorner.y;
	const float min_z = req.bottomLeftCorner.z;

	cerr << "min [" << min_x << ", " << min_y << ", " << min_z << "] " << endl;
	cerr << "max [" << max_x << ", " << max_y << ", " << max_z << "] " << endl;



	tf::StampedTransform stampedTr;
	
	Eigen::Affine3d eigenTr;
	tf::poseMsgToEigen(req.mapCenter, eigenTr);
	Eigen::MatrixXf T = eigenTr.matrix().inverse().cast<float>();
	//const Eigen::MatrixXf T = eigenTr.matrix().cast<float>();

	cerr << "T:" << endl << T << endl;
	T = transformation->correctParameters(T);

		
	// FIXME: do we need a mutex here?
	const DP centeredPointCloud = transformation->compute(*mapPointCloud, T); 
	DP cutPointCloud = centeredPointCloud.createSimilarEmpty();

	cerr << centeredPointCloud.features.topLeftCorner(3, 10) << endl;
	cerr << T << endl;

	int newPtCount = 0;
	for(int i=0; i < centeredPointCloud.features.cols(); i++)
	{
		const float x = centeredPointCloud.features(0,i);
		const float y = centeredPointCloud.features(1,i);
		const float z = centeredPointCloud.features(2,i);
		
		if(x < max_x && x > min_x &&
			 y < max_y && y > min_y &&
		   z < max_z && z > min_z 	)
		{
			cutPointCloud.setColFrom(newPtCount, centeredPointCloud, i);
			newPtCount++;	
		}
	}

	cerr << "Extract " << newPtCount << " points from the map" << endl;
	
	cutPointCloud.conservativeResize(newPtCount);
	cutPointCloud = transformation->compute(cutPointCloud, T.inverse()); 

	
	// Send the resulting point cloud in ROS format
	res.boundedMap = PointMatcher_ros::pointMatcherCloudToRosMsg<float>(cutPointCloud, mapFrame, ros::Time::now());
	return true;
}
controller::controller(ODEBodies * body_bag, float * root_position) {
    time = 0;
    T = 100;

    this->root_position = root_position;

    swingStart[0] = 1;
    swingEnd[0] = 50;
    swingStart[1] = 51;
    swingEnd[1] = 100;
    swingStart[2] = 1;
    swingEnd[2] = 50;
    swingStart[3] = 51;
    swingEnd[3] = 100;

    shoulderHeight = 470;		//in 0.1cm
    hipHeight = 450;

    current_velocity[0] = 0;		//in 0.1cm/s
    current_velocity[1] = 0;
    current_velocity[2] = 0;
    desired_velocity[0] = -1000;
    desired_velocity[1] = 0;
    desired_velocity[2] = 0;

    lfHeight[0] = shoulderHeight;
    lfHeight[1] = shoulderHeight;
    lfHeight[2] = hipHeight;
    lfHeight[3] = hipHeight;

    for(int i = 0; i < 4; i++) {
        swingFlag[i] = false;
        for(int j = 0; j < 4; j++) {
            foot_link_pd_error[i][j] = 0;
        }
        foot_link_gain_kp[i][0] = 1000;
        foot_link_gain_kd[i][0] = 1000;
        foot_link_gain_kp[i][1] = 1000;
        foot_link_gain_kd[i][1] = 1000;
        foot_link_gain_kp[i][2] = 100;
        foot_link_gain_kd[i][2] = 100;
        foot_link_gain_kp[i][3] = -700;
        foot_link_gain_kd[i][3] = 10;
        for(int j = 0; j < 3; j++) {
            swing_torque[i][j] = 0;
        }
    }

    lf_velocity_force_kv[0] = 0.01;
    lf_velocity_force_kv[1] = 0.01;

    this->body_bag = body_bag;

    for(int i = 0; i< 4; i++) {
        //Alignment from my co-ordinate system to ODE system
        Eigen::MatrixXf alignment = Eigen::MatrixXf::Identity(4, 4);
        alignment(1, 1) = 0;
        alignment(1, 2) = 1;
        alignment(2, 1) = -1;
        alignment(2, 2) = 0;

        const dReal *front_left_foot_link_4_location = dBodyGetPosition(body_bag->getFootLinkBody(i,3));

        //translation
        Eigen::MatrixXf mt = Eigen::MatrixXf::Identity(4, 4);
        mt(1, 3) = body_bag->getFootLinkLength(i, 3)/2;

        //rotation
        Eigen::MatrixXf mr = Eigen::MatrixXf::Identity(4, 4);
        const dReal *rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(i, 3));
        for(int k = 0; k < 3; k++) {
            for(int j = 0; j < 4; j++) {
                mr(k, j) = rotation_matrix_ode[k*4+j];
            }
        }
        mr(3, 0) = 0;
        mr(3, 1) = 0;
        mr(3, 2) = 0;
        mr(3, 3) = 1;

        Eigen::MatrixXf origin = Eigen::MatrixXf::Random(4, 1);
        origin(0, 0) = 0;
        origin(1, 0) = 0;
        origin(2, 0) = 0;
        origin(3, 0) = 1;

        Eigen::MatrixXf addition = alignment.inverse()*mr.inverse()*mt*origin;
        prev_stepping_location[i][0] = front_left_foot_link_4_location[0] + addition(0, 0);
        prev_stepping_location[i][1] = front_left_foot_link_4_location[1] + addition(1, 0);
        prev_stepping_location[i][2] = front_left_foot_link_4_location[2] + addition(2, 0);
        next_stepping_location[i][0] = prev_stepping_location[i][0];
        next_stepping_location[i][1] = prev_stepping_location[i][1];
        next_stepping_location[i][2] = prev_stepping_location[i][2];
        current_foot_location[i][0] = prev_stepping_location[i][0];
        current_foot_location[i][1] = prev_stepping_location[i][1];
        current_foot_location[i][2] = prev_stepping_location[i][2];
    }
}
void controller::legController(int leg_id, int phase) {
    cout << endl << "Leg Controller = " << leg_id << endl;
    if(swingStart[leg_id] == phase) {
        cout << "Starting swing phase"<< endl;
        computePlacementLocation(leg_id, lfHeight[0]);
        setFootLocation(leg_id, phase);
    }
    else if(isInSwing(leg_id)) {
        cout << "Swing phase"<< endl;
        swingFlag[leg_id] = true;

        //Get target position
        setFootLocation(leg_id, phase);
        vector<float> targetPosition = getTargetPosition(leg_id);

        //Get link lengths
        vector<float> lengths(2);
        for(int i = 0; i < 2; i++) {
            lengths[i] = body_bag->getFootLinkLength(leg_id, i);
        }
        //Set axis perpendicular to the kinematic chain plane
        Eigen::MatrixXf axis = Eigen::MatrixXf::Random(4, 1);
        axis(0, 0) = 0;
        axis(1, 0) = 0;
        if(leg_id % 2 == 0) {
            axis(2, 0) = 1;
        }
        else {
            axis(2, 0) = -1;
        }
        axis(3, 0) = 1;

        //Get transformation matrices
        vector<Eigen::MatrixXf> transformationMatrices(2);
        Eigen::MatrixXf alignment = Eigen::MatrixXf::Identity(4, 4);
        alignment(1, 1) = 0;
        alignment(1, 2) = 1;
        alignment(2, 1) = -1;
        alignment(2, 2) = 0;

        Eigen::MatrixXf mr = Eigen::MatrixXf::Identity(4, 4);
        const dReal *rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
        for(int i = 0; i < 3; i++) {
            for(int j = 0; j < 4; j++) {
                mr(i, j) = rotation_matrix_ode[i*4+j];
            }
        }
        mr(3, 0) = 0;
        mr(3, 1) = 0;
        mr(3, 2) = 0;
        mr(3, 3) = 1;

        Eigen::MatrixXf mr2 = Eigen::MatrixXf::Identity(4, 4);
        const dReal *rotation_matrix_ode2 = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
        for(int i = 0; i < 3; i++) {
            for(int j = 0; j < 4; j++) {
                mr2(i, j) = rotation_matrix_ode2[i*4+j];
            }
        }
        mr2(3, 0) = 0;
        mr2(3, 1) = 0;
        mr2(3, 2) = 0;
        mr2(3, 3) = 1;

        transformationMatrices[0] = mr*alignment.inverse();
        transformationMatrices[1] = mr2*alignment.inverse();
        //Get translation matrix
        const dReal *center_location = dBodyGetPosition(body_bag->getFootLinkBody(leg_id,0)); //get location of the center of the link
        Eigen::MatrixXf mt = Eigen::MatrixXf::Identity(4, 4); //translate to the start of the link
        mt(1, 3) = -body_bag->getFootLinkLength(leg_id, 0)/2;
        mr = Eigen::MatrixXf::Identity(4, 4); //get orientation
        rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 0));
        for(int k = 0; k < 3; k++) {
            for(int j = 0; j < 4; j++) {
                mr(k, j) = rotation_matrix_ode[k*4+j];
            }
        }
        mr(3, 0) = 0;
        mr(3, 1) = 0;
        mr(3, 2) = 0;
        mr(3, 3) = 1;
        Eigen::MatrixXf origin = Eigen::MatrixXf::Random(4, 1);
        origin(0, 0) = 0;
        origin(1, 0) = 0;
        origin(2, 0) = 0;
        origin(3, 0) = 1;
        Eigen::MatrixXf addition = alignment.inverse()*mr.inverse()*mt*origin; //part to add to the center location
        Eigen::MatrixXf translationMatrix = Eigen::MatrixXf::Identity(4, 4);
        translationMatrix(0, 3) = center_location[0] + addition(0, 0);
        translationMatrix(1, 3) = center_location[1] + addition(1, 0);
        translationMatrix(2, 3) = center_location[2] + addition(2, 0);

        vector<float> angles = body_bag->getAngles(0);
        Eigen::MatrixXf jointAngleChange = applyIK(lengths, transformationMatrices, angles, targetPosition, translationMatrix, axis);

        //Use PD controllers to get torque
        //link 1
        float torque = foot_link_gain_kp[leg_id][0]*jointAngleChange(0,0) + foot_link_gain_kd[leg_id][0]*(jointAngleChange(0,0) - foot_link_pd_error[leg_id][0]);
        dBodyAddTorque(body_bag->getFootLinkBody(leg_id,0), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
        for(int i = 0; i < 3; i++) {
            swing_torque[leg_id][i] = axis(i, 0)*torque;
        }
        foot_link_pd_error[leg_id][0] = jointAngleChange(0,0);

        //link 2
        torque = foot_link_gain_kp[leg_id][1]*jointAngleChange(1,0) + foot_link_gain_kd[leg_id][1]*(jointAngleChange(1,0) - foot_link_pd_error[leg_id][1]);
        dBodyAddTorque(body_bag->getFootLinkBody(leg_id,1), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
        foot_link_pd_error[leg_id][1] = jointAngleChange(1,0);

        //link 3
        rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 2));
        for(int k = 0; k < 3; k++) {
            for(int j = 0; j < 4; j++) {
                mr(k, j) = rotation_matrix_ode[k*4+j];
            }
        }
        mr(3, 0) = 0;
        mr(3, 1) = 0;
        mr(3, 2) = 0;
        mr(3, 3) = 1;
        float swing_phase = computeSwingPhase(leg_id, phase);
        float error = ((30*M_PI)/180)*(1 - swing_phase) - ((60*M_PI)/180)*swing_phase - acos(mr(0, 0));
        torque = foot_link_gain_kp[leg_id][2]*error + foot_link_gain_kd[leg_id][2]*(error - foot_link_pd_error[leg_id][2]);
        dBodyAddTorque(body_bag->getFootLinkBody(leg_id,2), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
        foot_link_pd_error[leg_id][2] = error;

        //link 4
        rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
        for(int k = 0; k < 3; k++) {
            for(int j = 0; j < 4; j++) {
                mr(k, j) = rotation_matrix_ode[k*4+j];
            }
        }
        mr(3, 0) = 0;
        mr(3, 1) = 0;
        mr(3, 2) = 0;
        mr(3, 3) = 1;
        swing_phase = computeSwingPhase(leg_id, phase);
        error = ((90*M_PI)/180)*(1 - swing_phase) - ((30*M_PI)/180)*swing_phase - acos(mr(0, 0));
        torque = foot_link_gain_kp[leg_id][3]*error + foot_link_gain_kd[leg_id][3]*(error - foot_link_pd_error[leg_id][3]);
        dBodyAddTorque(body_bag->getFootLinkBody(leg_id,3), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
        foot_link_pd_error[leg_id][3] = error;


        //Apply gravity compensation
        float force = 9.810*body_bag->getDensity()*(body_bag->getFootLinkLength(leg_id, 0) + body_bag->getFootLinkLength(leg_id, 1) + body_bag->getFootLinkLength(leg_id, 2) + body_bag->getFootLinkLength(leg_id, 3));

        dBodyAddForce(body_bag->getFootLinkBody(leg_id, 2), 0.0, 9.810*body_bag->getDensity()*body_bag->getFootLinkLength(leg_id, 2), 0.0);

        if(leg_id < 2) {
            dBodyAddForce(body_bag->getBackLink1Body(), 0.0, force, 0.0);
        }
        else {
            dBodyAddForce(body_bag->getBackLink6Body(), 0.0, force, 0.0);
        }
    }
    else {
        cout << "Stance phase"<< endl;
        swingFlag[leg_id] = false;
        /*for(int i = 0; i < 3; i++){
        	swing_torque[leg_id][i] = 0;
        }*/
        stanceLegTreatment(leg_id);
    }
}
int main(int argc, char* argv[])
{
#if 1
	QApplication a(argc, argv);
	MainWindow w;
	w.show();


	// look for image file
	//std::string img_file("data/floor.jpg");
	//QFile file;
	//for (int i = 0; i < 5; ++i)
	//	if (!file.exists(img_file.c_str()))
	//		img_file.insert(0, "../");
	//w.getGLWidget()->setImage(img_file.c_str());

	

	GLLines* gllines = &w.getGLWidget()->glLines();

	
	//gllines->setVertexLine(0, 0, QVector3D(52.3467f, 125.102f, 1.0f));
	//gllines->setVertexLine(0, 1, QVector3D(340.253f, 130.147f, 1.0f));
	//gllines->setVertexLine(1, 0, QVector3D(193.28f, 126.111f, 1.0f));
	//gllines->setVertexLine(1, 1, QVector3D(225.493f, 360.173f, 1.0f));
	//gllines->setVertexLine(2, 0, QVector3D(42.28f, 263.32f, 1.0f));
	//gllines->setVertexLine(2, 1, QVector3D(296.967f, 397.502f, 1.0f));
	//gllines->setVertexLine(3, 0, QVector3D(212.407f, 269.373f, 1.0f));
	//gllines->setVertexLine(3, 1, QVector3D(34.2267f, 391.449f, 1.0f));
	//gllines->setVertexLine(4, 0, QVector3D(294.953f, 318.809f, 1.0f));
	//gllines->setVertexLine(4, 1, QVector3D(456.02f, 322.844f, 1.0f));
	//gllines->setVertexLine(5, 0, QVector3D(492.26f, 208.84f, 1.0f));
	//gllines->setVertexLine(5, 1, QVector3D(429.847f, 400.529f, 1.0f));
	//gllines->setVertexLine(6, 0, QVector3D(299.987f, 31.2756f, 1.0f));
	//gllines->setVertexLine(6, 1, QVector3D(555.68f, 273.409f, 1.0f));
	//gllines->setVertexLine(7, 0, QVector3D(545.613f, 39.3467f, 1.0f));
	//gllines->setVertexLine(7, 1, QVector3D(236.567f, 250.204f, 1.0f));
	//gllines->setVertexLine(8, 0, QVector3D(95.6333f, 264.329f, 1.0f));
	//gllines->setVertexLine(8, 1, QVector3D(501.32f, 273.409f, 1.0f));
	//gllines->setVertexLine(9, 0, QVector3D(302.00f, 29.2578f, 1.0f));
	//gllines->setVertexLine(9, 1, QVector3D(297.973f, 398.511f, 1.0f));
	gllines->computeCanonicalVertices(w.getGLWidget()->width(), w.getGLWidget()->height());
	gllines->onEnable(true);

	return a.exec();

#else
	std::vector<Eigen::Vector3f> vertices;
	vertices.push_back(Eigen::Vector3f(52.3467f, 125.102f, 1.0f));
	vertices.push_back(Eigen::Vector3f(340.253f, 130.147f, 1.0f));
	vertices.push_back(Eigen::Vector3f(193.28f, 126.111f, 1.0f));
	vertices.push_back(Eigen::Vector3f(225.493f, 360.173f, 1.0f));
	vertices.push_back(Eigen::Vector3f(42.28f, 263.32f, 1.0f));
	vertices.push_back(Eigen::Vector3f(296.967f, 397.502f, 1.0f));
	vertices.push_back(Eigen::Vector3f(212.407f, 269.373f, 1.0f));
	vertices.push_back(Eigen::Vector3f(34.2267f, 391.449f, 1.0f));
	vertices.push_back(Eigen::Vector3f(294.953f, 318.809f, 1.0f));
	vertices.push_back(Eigen::Vector3f(456.02f, 322.844f, 1.0f));
	vertices.push_back(Eigen::Vector3f(492.26f, 208.84f, 1.0f));
	vertices.push_back(Eigen::Vector3f(429.847f, 400.529f, 1.0f));
	vertices.push_back(Eigen::Vector3f(299.987f, 31.2756f, 1.0f));
	vertices.push_back(Eigen::Vector3f(555.68f, 273.409f, 1.0f));
	vertices.push_back(Eigen::Vector3f(545.613f, 39.3467f, 1.0f));
	vertices.push_back(Eigen::Vector3f(236.567f, 250.204f, 1.0f));
	vertices.push_back(Eigen::Vector3f(95.6333f, 264.329f, 1.0f));
	vertices.push_back(Eigen::Vector3f(501.32f, 273.409f, 1.0f));
	vertices.push_back(Eigen::Vector3f(302.00f, 29.2578f, 1.0f));
	vertices.push_back(Eigen::Vector3f(297.973f, 398.511f, 1.0f));


	std::cout << vertices.size() << std::endl;

	std::vector<Eigen::Vector3f> lines;
	for (int i = 0; i < vertices.size() - 1; i += 2)
		lines.push_back(lineNormalized(vertices[i], vertices[i + 1]));
	
	for (int i = 0; i < lines.size(); ++i)
		std::cout << "l" << i << " : " << lines[i].x() << ", " << lines[i].y() << ", " << lines[i].z() << std::endl;

	std::cout << std::endl << std::endl;

	//lines[0] = Eigen::Vector3f(-0.000141084, 0.00805224, -0.999968);
	//lines[1] = Eigen::Vector3f(-0.00568419, 0.000782299, 0.999984);
	//lines[2] = Eigen::Vector3f(-0.00218568, 0.00414856, -0.999989);
	//lines[3] = Eigen::Vector3f(-0.0016513, -0.00241022, 0.999996);
	//lines[4] = Eigen::Vector3f(-8.04546e-05, 0.00321109, -0.999995);
	//lines[5] = Eigen::Vector3f(-0.00178489, -0.000581155, 0.999998);
	//lines[6] = Eigen::Vector3f(-0.00374583, 0.0039556, 0.999985);
	//lines[7] = Eigen::Vector3f(-0.00165759, -0.00242947, 0.999996);
	//lines[8] = Eigen::Vector3f(-8.53647e-05, 0.00381402, -0.999993);
	//lines[9] = Eigen::Vector3f(-0.00330775, -3.60706e-05, 0.999995);


	Eigen::MatrixXf A(5, 5);
	Eigen::Vector3f l, m;
	Eigen::VectorXf b(5);

	for (int i = 0; i < 5; ++i)
	{
		l = lines[i * 2 + 0];
		m = lines[i * 2 + 1];

		A(i, 0) = l[0] * m[0];
		A(i, 1) = (l[0] * m[1] + l[1] * m[0]) / 2.0f;
		A(i, 2) = l[1] * m[1];
		A(i, 3) = (l[0] * m[2] + l[2] * m[0]) / 2.0f;
		A(i, 4) = (l[1] * m[2] + l[2] * m[1]) / 2.0f;
		A(i, 5) = l[2] * m[2];

		b[i] = -l[2] * m[2];
	}

	std::cout << "A: \n" << A << std::endl << std::endl;
	std::cout << "b: \n" << b << std::endl << std::endl;

	Eigen::MatrixXf x = A.colPivHouseholderQr().solve(b);
	std::cout << std::fixed << "x: \n" << x << std::endl << std::endl;


	Eigen::MatrixXf conic(3, 3);
	conic(0, 0) = x(0);
	conic(0, 1) = x(1) / 2.0f;
	conic(0, 2) = x(3) / 2.0f;
	conic(1, 0) = x(1) / 2.0f;
	conic(1, 1) = x(2);
	conic(1, 2) = x(4) / 2.0f;
	conic(2, 0) = x(3) / 2.0f;
	conic(2, 1) = x(4) / 2.0f;
	conic(2, 2) = 1.0f;
	std::cout << "Conic : " << std::endl << conic << std::endl << std::endl;

	Eigen::JacobiSVD<Eigen::MatrixXf> svd(conic, Eigen::ComputeFullU);
	Eigen::MatrixXf H = svd.matrixU();
	
	std::cout << "H matrix: " << std::endl 
		<< H << std::endl << std::endl 
		<< "Singular values: " << svd.singularValues()
		<< std::endl << std::endl;

	std::cout << "Rectification transformation: " << std::endl << H.inverse() << std::endl << std::endl;





	QImage input("floor-persp.jpg");
	Eigen::Vector3f img(input.width(), input.height(), 1.0f);

	float xmin = 0;
	float xmax = 0;
	float ymin = 0;
	float ymax = 0;
	computImageSize(H.inverse(), 0, 0, input.width(), input.height(), xmin, xmax, ymin, ymax);


	float aspect = (xmax - xmin) / (ymax - ymin);
	QImage output(input.width(), input.width() / aspect, input.format());
	output.fill(qRgb(0, 0, 0));

	std::cout << "Output size: " << output.width() << ", " << output.height() << std::endl;

	float dx = (xmax - xmin) / float(output.width());
	float dy = (ymax - ymin) / float(output.height());

	std::cout << std::fixed << "dx, dy: " << dx << ", " << dy << std::endl;

	for (int x = 0; x < output.width(); ++x)
	{
		for (int y = 0; y < output.height(); ++y)
		{
			Eigen::Vector3f px(x, y, 1);

			float tx = 0.0f;
			float ty = 0.0f;
			Eigen::Vector2f t = multiplyPointMatrix(H, xmin + x * dx, ymin + y * dy);

			if (t.x() > -1 && t.y() > -1
				&& t.x() < input.width()
				&& t.y() < input.height())
			{
				//if (interpolate)
				//{
				//	QRgb rgb = bilinearInterpol(input, t.x(), t.y(), dx / 2.0, dy / 2.0);
				//	output.setPixel(x, y, rgb);
				//}
				//else
				{
					output.setPixel(x, y, input.pixel(t.x(), t.y()));
				}
			}
		}
	}


	output.save("output_5_floor.jpg");
	
	return EXIT_SUCCESS;
#endif
}
Esempio n. 11
0
void SingleParticle2dx::DataStructures::Particle::calculateConsistency()
{
	size_type n = 6;
	std::vector<Eigen::VectorXf> points;
	std::vector<Particle*> neighbors = getNeighbors();
	
	for (size_type i=0; i<static_cast<size_type>(neighbors.size()); i++ )
	{
		Eigen::VectorXf new_vec(n);
		new_vec[0] = neighbors[i]->getNewOrientation().getTLTAXIS();
		new_vec[1] = neighbors[i]->getNewOrientation().getTLTANG();
		new_vec[2] = neighbors[i]->getNewOrientation().getTAXA();
		new_vec[3] = neighbors[i]->getParticleShift().getShiftX();
		new_vec[4] = neighbors[i]->getParticleShift().getShiftY();
		new_vec[5] = neighbors[i]->getSimMeasure();
		points.push_back(new_vec);
	}
	
	Eigen::VectorXf mean(n);
	
	for(size_type i=0; i<static_cast<size_type>(points.size()); i++)
	{
		mean += points[i];
	}
	mean /= static_cast<value_type>(points.size());
	
//	std::cout << ": mean = " << mean[0] << " " << mean[1] << " " << mean[2] << " " << mean[3] << " " << mean[4] << " " << mean[5] << std::endl;
	
	Eigen::MatrixXf covMat = Eigen::MatrixXf::Zero(n,n);
	
//	std::cout << mean << std::endl;
	
	for(size_type i=0; i<static_cast<size_type>(points.size()); i++)
	{
		Eigen::VectorXf diff = (points[i]-mean).conjugate();
		covMat += diff * diff.adjoint();
	}
	
//	std::cout << ":det = " << covMat.determinant() << std::endl;
	
	value_type det = covMat.determinant();
	
	if ( !std::isfinite(det) )
	{
		setConsistency(0);
		return;
	}
	
	if ( det < 0.001 )
	{
		setConsistency(0);
		return;
	}
	
	//SingleParticle2dx::Utilities::UtilityFunctions::reqularizeMatrix(covMat);
	
//	std::cout << covMat << std::endl;
	
	Eigen::VectorXf vec(n);
	vec[0] = getNewOrientation().getTLTAXIS();
	vec[1] = getNewOrientation().getTLTANG();
	vec[2] = getNewOrientation().getTAXA();
	vec[3] = getParticleShift().getShiftX();
	vec[4] = getParticleShift().getShiftY();
	vec[5] = getSimMeasure();
	
	value_type result = -0.5 * log(covMat.determinant());
	
//	#pragma omp critical (det_output)
	//{
		//std::cout << ":det = " << det << std::endl;
	//}
	
	if ( covMat.determinant() < 0.000001 )
	{
		setConsistency(0);
		return;
	}
	
//	std::cout << ":first term: " << result << std::endl;
	
	Eigen::VectorXf tmp1 = (covMat.inverse()) * (vec-mean);
	value_type tmp2 = (vec-mean).dot(tmp1);
	
	result -= 0.5 * tmp2;
	
//	std::cout << ":second term: " << -0.5 * tmp2 << std::endl;

	if ( boost::math::isnan(result) || boost::math::isnan(-result) )
	{
		std::cout << "::reset CONS realy done now" << std::endl;
		result = 0;
	}
	
	setConsistency(result);
}