C++ MatrixXf::inverse方法代码示例

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

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

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

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

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++) {
//.........这里部分代码省略.........

//.........这里部分代码省略.........
		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
}

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