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