/**
* @brief CameraDirectLinearTransformation::rq3
* Perform 3 RQ decomposition of matrix A and save them in matrix R and matrix Q
* http://www.csse.uwa.edu.au/~pk/research/matlabfns/Projective/rq3.m
* @param A
* @param R
* @param Q
*/
void CameraDirectLinearTransformation::rq3(const Matrix3d &A, Matrix3d &R, Matrix3d& Q)
{
// Find rotation Qx to set A(2,1) to 0
double c = -A(2,2)/sqrt(A(2,2)*A(2,2)+A(2,1)*A(2,1));
double s = A(2,1)/sqrt(A(2,2)*A(2,2)+A(2,1)*A(2,1));
Matrix3d Qx,Qy,Qz;
Qx << 1 ,0,0, 0,c,-s, 0,s,c;
R = A*Qx;
// Find rotation Qy to set A(2,0) to 0
c = R(2,2)/sqrt(R(2,2)*R(2,2)+R(2,0)*R(2,0) );
s = R(2,0)/sqrt(R(2,2)*R(2,2)+R(2,0)*R(2,0) );
Qy << c, 0, s, 0, 1, 0,-s, 0, c;
R*=Qy;
// Find rotation Qz to set A(1,0) to 0
c = -R(1,1)/sqrt(R(1,1)*R(1,1)+R(1,0)*R(1,0));
s = R(1,0)/sqrt(R(1,1)*R(1,1)+R(1,0)*R(1,0));
Qz << c ,-s, 0, s ,c ,0, 0, 0 ,1;
R*=Qz;
Q = Qz.transpose()*Qy.transpose()*Qx.transpose();
// Adjust R and Q so that the diagonal elements of R are +ve
for (int n=0; n<3; n++)
{
if (R(n,n)<0)
{
R.col(n) = - R.col(n);
Q.row(n) = - Q.row(n);
}
}
}
void EdgeProjectXYZ2UVQ::linearizeOplus()
{
VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
SE3Quat T(vj->estimate());
VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
Vector3d xyz = vi->estimate();
Vector3d xyz_trans = T.map(xyz);
const double & x = xyz_trans[0];
const double & y = xyz_trans[1];
const double & z = xyz_trans[2];
double z_sq = z*z;
const double & Fx = vj->_focal_length(0);
const double & Fy = vj->_focal_length(1);
double dq_dz = -Fx/z_sq;
double x_Fx_by_zsq = x*Fx/z_sq;
double y_Fy_by_zsq = y*Fy/z_sq;
Matrix3d R = T.rotation().toRotationMatrix();
_jacobianOplusXi.row(0) = -Fx/z*R.row(0) + x_Fx_by_zsq*R.row(2);
_jacobianOplusXi.row(1) = -Fy/z*R.row(1) + y_Fy_by_zsq*R.row(2);
_jacobianOplusXi.row(2) = -dq_dz*R.row(2);
_jacobianOplusXj(0,0) = x*y/z_sq *Fx;
_jacobianOplusXj(0,1) = -(1+(x*x/z_sq)) *Fx;
_jacobianOplusXj(0,2) = y/z *Fx;
_jacobianOplusXj(0,3) = -1./z *Fx;
_jacobianOplusXj(0,4) = 0;
_jacobianOplusXj(0,5) = x/z_sq *Fx;
_jacobianOplusXj(1,0) = (1+y*y/z_sq) *Fy;
_jacobianOplusXj(1,1) = -x*y/z_sq *Fy;
_jacobianOplusXj(1,2) = -x/z *Fy;
_jacobianOplusXj(1,3) = 0;
_jacobianOplusXj(1,4) = -1./z *Fy;
_jacobianOplusXj(1,5) = y/z_sq *Fy;
_jacobianOplusXj(2,0) = -y*dq_dz ;
_jacobianOplusXj(2,1) = x*dq_dz;
_jacobianOplusXj(2,2) = 0;
_jacobianOplusXj(2,3) = 0;
_jacobianOplusXj(2,4) = 0;
_jacobianOplusXj(2,5) = -dq_dz ;
// std::cerr << _jacobianOplusXi << std::endl;
// std::cerr << _jacobianOplusXj << std::endl;
// BaseBinaryEdge<3, Vector3d, VertexPointXYZ, VertexSE3Expmap, false>::linearizeOplus();
// std::cerr << _jacobianOplusXi << std::endl;
// std::cerr << _jacobianOplusXj << std::endl;
}
void Camera::initializeViewPoint()
{
d->modelview.setIdentity();
d->orthoScale = 1.0;
if( d->parent == 0 )
return;
if( d->parent->molecule() == 0 )
return;
// if the molecule is empty, we want to look at its center
// (which is probably at the origin, but who knows) from some distance
// (here 20.0) -- this gives us some room to work PR#1964674
if( d->parent->molecule()->numAtoms() < 2 &&
d->parent->molecule()->OBUnitCell() == NULL)
{
d->modelview.translate(-d->parent->center() - Vector3d(0.0, 0.0, 20.0));
return;
}
// if we're here, the molecule is not empty, i.e. has atoms.
// we want a top-down view on it, i.e. the molecule should fit as well as
// possible in the (X,Y)-plane. Equivalently, we want the Z axis to be parallel
// to the normal vector of the molecule's fitting plane.
// Thus we construct a suitable base-change rotation.
Matrix3d rotation;
rotation.row(2) = d->parent->normalVector();
rotation.row(0) = rotation.row(2).unitOrthogonal();
rotation.row(1) = rotation.row(2).cross(rotation.row(0));
// set the camera's matrix to be (the 4x4 version of) this rotation.
d->modelview.linear() = rotation;
// now we want to move backwards, in order
// to view the molecule from a distance, not from inside it.
// This translation must be applied after the above rotation, so we
// want a left-multiplication here. Whence pretranslate().
pretranslate( - 3.0 * ( d->parent->radius() + CAMERA_NEAR_DISTANCE ) *
Vector3d::UnitZ() );
// the above rotation is meant to be a rotation around the molecule's
// center. So before this rotation is applied, the molecule's center
// must be brought to the origin of the coordinate systemby a translation.
// As this translation must be applied first, we want a right-multiplication here.
// Whence translate().
translate( - d->parent->center() );
}
int main(int, char**)
{
cout.precision(3);
Matrix3d m = Matrix3d::Identity();
m.row(1) = Vector3d(4,5,6);
cout << m << endl;
return 0;
}
void CameraDirectLinearTransformation::decomposePMatrix(const Eigen::Matrix<double,3,4> &P)
{
Matrix3d M = P.topLeftCorner<3,3>();
Vector3d m3 = M.row(2).transpose();
// Follow the HartleyZisserman - "Multiple view geometry in computer vision" implementation chapter 3
Matrix3d P123,P023,P013,P012;
P123 << P.col(1),P.col(2),P.col(3);
P023 << P.col(0),P.col(2),P.col(3);
P013 << P.col(0),P.col(1),P.col(3);
P012 << P.col(0),P.col(1),P.col(2);
double X = P123.determinant();
double Y = -P023.determinant();
double Z = P013.determinant();
double T = -P012.determinant();
C << X/T,Y/T,Z/T;
// Image Principal points computed with homogeneous normalization
this->principalPoint = (M*m3).eval().hnormalized().head<2>();
// Principal vector from the camera centre C through pp pointing out from the camera. This may not be the same as R(:,3)
// if the principal point is not at the centre of the image, but it should be similar.
this->principalVector = (M.determinant()*m3).normalized();
this->R = this->K = Matrix3d::Identity();
this->rq3(M,this->K,this->R);
// To understand how K is formed http://ksimek.github.io/2013/08/13/intrinsic/
// and also read http://ksimek.github.io/2012/08/14/decompose/
K/=K(2,2); // EXTREMELY IMPORTANT TO MAKE THE K(2,2)==1 !!!
// K = [ fx, s, x0;
// 0, fy, y0;
// 0, 0, 1];
// Where fx is the focal length on x measured in pixels, fy is the focal length ony measured in pixels
// Negate the second column of K and R because the y window coordinates and camera y direction are opposite is positive
// This is the solution I've found personally to correct the behaviour using OpenGL gluPerspective convention
this->R.row(2)=-R.row(2);
// Our 3x3 intrinsic camera matrix K needs two modifications before it's ready to use in OpenGL. First, for proper clipping, the (3,3) element of K must be -1. OpenGL's camera looks down the negative z-axis, so if K33 is positive, vertices in front of the camera will have a negative w coordinate after projection. In principle, this is okay, but because of how OpenGL performs clipping, all of these points will be clipped.
//this->K.col(2) = -K.col(2);
// t is the location of the world origin in camera coordinates.
t = -R*C;
}
void DecomposeEssentialUsingHorn90(double _E[9], double _R1[9], double _R2[9], double _t1[3], double _t2[3]) {
//from : http://people.csail.mit.edu/bkph/articles/Essential.pdf
#ifdef USE_EIGEN
using namespace Eigen;
Matrix3d E = Map<Matrix<double,3,3,RowMajor> >(_E);
Matrix3d EEt = E * E.transpose();
Vector3d e0e1 = E.col(0).cross(E.col(1)),e1e2 = E.col(1).cross(E.col(2)),e2e0 = E.col(2).cross(E.col(0));
Vector3d b1,b2;
#if 1
//Method 1
Matrix3d bbt = 0.5 * EEt.trace() * Matrix3d::Identity() - EEt; //Horn90 (12)
Vector3d bbt_diag = bbt.diagonal();
if (bbt_diag(0) > bbt_diag(1) && bbt_diag(0) > bbt_diag(2)) {
b1 = bbt.row(0) / sqrt(bbt_diag(0));
b2 = -b1;
} else if (bbt_diag(1) > bbt_diag(0) && bbt_diag(1) > bbt_diag(2)) {
b1 = bbt.row(1) / sqrt(bbt_diag(1));
b2 = -b1;
} else {
b1 = bbt.row(2) / sqrt(bbt_diag(2));
b2 = -b1;
}
#else
//Method 2
if (e0e1.norm() > e1e2.norm() && e0e1.norm() > e2e0.norm()) {
b1 = e0e1.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
} else if (e1e2.norm() > e0e1.norm() && e1e2.norm() > e2e0.norm()) {
b1 = e1e2.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
} else {
b1 = e2e0.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
}
#endif
//Horn90 (19)
Matrix3d cofactors; cofactors.col(0) = e1e2; cofactors.col(1) = e2e0; cofactors.col(2) = e0e1;
cofactors.transposeInPlace();
//B = [b]_x , see Horn90 (6) and http://en.wikipedia.org/wiki/Cross_product#Conversion_to_matrix_multiplication
Matrix3d B1; B1 << 0,-b1(2),b1(1),
b1(2),0,-b1(0),
-b1(1),b1(0),0;
Matrix3d B2; B2 << 0,-b2(2),b2(1),
b2(2),0,-b2(0),
-b2(1),b2(0),0;
Map<Matrix<double,3,3,RowMajor> > R1(_R1),R2(_R2);
//Horn90 (24)
R1 = (cofactors.transpose() - B1*E) / b1.dot(b1);
R2 = (cofactors.transpose() - B2*E) / b2.dot(b2);
Map<Vector3d> t1(_t1),t2(_t2);
t1 = b1; t2 = b2;
cout << "Horn90 provided " << endl << R1 << endl << "and" << endl << R2 << endl;
#endif
}
void Omega3::AcquisitionTask::run(){
while(1){
//! qDebug()<< "try to open the first available device";
if (drdOpen () < 0) {
//qDebug()<<"no device available...";
//dhdSleep (2.0);
omegaDetected = false;
}
else{
omegaDetected = true;
}
if(omegaDetected){
break;
}
sleep(1);
}
done = 0;
double p[DHD_MAX_DOF];
double r[3][3];
double v[DHD_MAX_DOF];
double f[DHD_MAX_DOF];
double normf, normt;
double t0 = dhdGetTime ();
double t1 = t0;
bool base = false;
bool wrist = false;
bool grip = false;
//! Eigen objects (mapped to the arrays above)
Map<Vector3d> position(&p[0], 3);
Map<Vector3d> force (&f[0], 3);
Map<Vector3d> torque (&f[3], 3);
Map<Vector3d> velpos (&v[0], 3);
Map<Vector3d> velrot (&v[3], 3);
Matrix3d center;
Matrix3d rotation;
//! center of workspace
center.setIdentity (); // rotation (matrix)
double nullPose[DHD_MAX_DOF] = { 0.0, 0.0, 0.0, // translation
0.0, 0.0, 0.0, // rotation (joint angles)
0.0 }; // gripper
//! print out device identifier
if (!drdIsSupported ()) {
dhdSleep (2.0);
drdClose ();
}
//! perform auto-initialization
if (!drdIsInitialized () && drdAutoInit () < 0) {
qDebug()<<"error: auto-initialization failed"<<dhdErrorGetLastStr ();
dhdSleep (2.0);
}
else if (drdStart () < 0) {
printf ("error: regulation thread failed to start (%s)\n", dhdErrorGetLastStr ());
dhdSleep (2.0);
}
//! move to center
drdMoveTo (nullPose);
// request a null force (only gravity compensation will be applied)
// this will only apply to unregulated axis
for (int i=0; i<DHD_MAX_DOF; i++) f[i] = 0.0;
drdSetForceAndTorqueAndGripperForce (f);
// disable all axis regulation (but leave regulation thread running)
drdRegulatePos (base);
drdRegulateRot (wrist);
drdRegulateGrip (grip);
//! save current pos
drdGetPositionAndOrientation (p, r);
for (int i=0; i<3; i++) rotation.row(i) = Vector3d::Map(&r[i][0], 3);
this->omega->patientHandling->setOmegaPosition(p[0],p[1],p[2]);
long long cpt = 0;
// loop while the button is not pushed
while (!done) {
// synchronize with regulation thread
drdWaitForTick ();
// get position/orientation/gripper and update Eigen rotation matrix
drdGetPositionAndOrientation (p, r);
for (int i=0; i<3; i++) rotation.row(i) = Vector3d::Map(&r[i][0], 3);
if(cpt%31==0){
this->omega->patientHandling->setOmegaPosition(p[0],p[1],p[2]);
}
else{
cpt += 1;
}
// get position/orientation/gripper velocity
drdGetVelocity (v);
// compute base centering force
force = - KP * position;
// compute wrist centering torque
AngleAxisd deltaRotation (rotation.transpose() * center);
torque = rotation * (KR * deltaRotation.angle() * deltaRotation.axis());
// compute gripper centering force
f[6] = - KG * (p[6] - 0.015);
// scale force to a pre-defined ceiling
if ((normf = force.norm()) > MAXF) force *= MAXF/normf;
// scale torque to a pre-defined ceiling
if ((normt = torque.norm()) > MAXT) torque *= MAXT/normt;
// scale gripper force to a pre-defined ceiling
if (f[6] > MAXG) f[6] = MAXG;
if (f[6] < -MAXG) f[6] = -MAXG;
// add damping
force -= KVP * velpos;
torque -= KWR * velrot;
f[6] -= KVG * v[6];
// apply centering force with damping
if (drdSetForceAndTorqueAndGripperForce (f, -1) < DHD_NO_ERROR) {
printf ("error: cannot set force (%s)\n", dhdErrorGetLastStr ());
done = 1;
}
// local loop refresh rate
//count++;
// display refresh rate and position at 10Hz
t1 = drdGetTime ();
if ((t1-t0) > REFRESH_INTERVAL) {
// retrieve/compute information to display
// double freq = (double)count/(t1-t0)*1e-3;
// count = 0;
// t0 = t1;
// qDebug()<<freq<<cpt;
if (dhdGetButtonMask()) {
qDebug()<<"stop";
dhdClose ();
done = 1;
}
}
//qDebug()<<cpt<<t1;
//dhdSleep (0.1);
}
}
RigidBodySystem::StateVector<double> RigidBodySystem::dynamics(
const double& t, const RigidBodySystem::StateVector<double>& x,
const RigidBodySystem::InputVector<double>& u) const {
using namespace std;
using namespace Eigen;
eigen_aligned_unordered_map<const RigidBody*, Matrix<double, 6, 1> > f_ext;
// todo: make kinematics cache once and re-use it (but have to make one per
// type)
auto nq = tree->num_positions;
auto nv = tree->num_velocities;
auto num_actuators = tree->actuators.size();
auto q = x.topRows(nq);
auto v = x.bottomRows(nv);
auto kinsol = tree->doKinematics(q, v);
// todo: preallocate the optimization problem and constraints, and simply
// update them then solve on each function eval.
// happily, this clunkier version seems fast enough for now
// the optimization framework should support this (though it has not been
// tested thoroughly yet)
OptimizationProblem prog;
auto const& vdot = prog.AddContinuousVariables(nv, "vdot");
auto H = tree->massMatrix(kinsol);
Eigen::MatrixXd H_and_neg_JT = H;
VectorXd C = tree->dynamicsBiasTerm(kinsol, f_ext);
if (num_actuators > 0) C -= tree->B * u.topRows(num_actuators);
// loop through rigid body force elements
{
// todo: distinguish between AppliedForce and ConstraintForce
size_t u_index = 0;
for (auto const& f : force_elements) {
size_t num_inputs = f->getNumInputs();
VectorXd force_input(u.middleRows(u_index, num_inputs));
C -= f->output(t, force_input, kinsol);
u_index += num_inputs;
}
}
// apply joint limit forces
{
for (auto const& b : tree->bodies) {
if (!b->hasParent()) continue;
auto const& joint = b->getJoint();
if (joint.getNumPositions() == 1 &&
joint.getNumVelocities() ==
1) { // taking advantage of only single-axis joints having joint
// limits makes things easier/faster here
double qmin = joint.getJointLimitMin()(0),
qmax = joint.getJointLimitMax()(0);
// tau = k*(qlimit-q) - b(qdot)
if (q(b->position_num_start) < qmin)
C(b->velocity_num_start) -=
penetration_stiffness * (qmin - q(b->position_num_start)) -
penetration_damping * v(b->velocity_num_start);
else if (q(b->position_num_start) > qmax)
C(b->velocity_num_start) -=
penetration_stiffness * (qmax - q(b->position_num_start)) -
penetration_damping * v(b->velocity_num_start);
}
}
}
// apply contact forces
{
VectorXd phi;
Matrix3Xd normal, xA, xB;
vector<int> bodyA_idx, bodyB_idx;
if (use_multi_contact)
tree->potentialCollisions(kinsol, phi, normal, xA, xB, bodyA_idx,
bodyB_idx);
else
tree->collisionDetect(kinsol, phi, normal, xA, xB, bodyA_idx, bodyB_idx);
for (int i = 0; i < phi.rows(); i++) {
if (phi(i) < 0.0) { // then i have contact
// todo: move this entire block to a shared an updated "contactJacobian"
// method in RigidBodyTree
auto JA = tree->transformPointsJacobian(kinsol, xA.col(i), bodyA_idx[i],
0, false);
auto JB = tree->transformPointsJacobian(kinsol, xB.col(i), bodyB_idx[i],
0, false);
Vector3d this_normal = normal.col(i);
// compute the surface tangent basis
Vector3d tangent1;
if (1.0 - this_normal(2) < EPSILON) { // handle the unit-normal case
// (since it's unit length, just
// check z)
tangent1 << 1.0, 0.0, 0.0;
} else if (1 + this_normal(2) < EPSILON) {
tangent1 << -1.0, 0.0, 0.0; // same for the reflected case
} else { // now the general case
tangent1 << this_normal(1), -this_normal(0), 0.0;
tangent1 /= sqrt(this_normal(1) * this_normal(1) +
this_normal(0) * this_normal(0));
}
Vector3d tangent2 = this_normal.cross(tangent1);
Matrix3d R; // rotation into normal coordinates
R.row(0) = tangent1;
R.row(1) = tangent2;
R.row(2) = this_normal;
auto J = R * (JA - JB); // J = [ D1; D2; n ]
auto relative_velocity = J * v; // [ tangent1dot; tangent2dot; phidot ]
{
// spring law for normal force: fA_normal = -k*phi - b*phidot
// and damping for tangential force: fA_tangent = -b*tangentdot
// (bounded by the friction cone)
Vector3d fA;
fA(2) =
std::max<double>(-penetration_stiffness * phi(i) -
penetration_damping * relative_velocity(2),
0.0);
fA.head(2) =
-std::min<double>(
penetration_damping,
friction_coefficient * fA(2) /
(relative_velocity.head(2).norm() + EPSILON)) *
relative_velocity.head(2); // epsilon to avoid divide by zero
// equal and opposite: fB = -fA.
// tau = (R*JA)^T fA + (R*JB)^T fB = J^T fA
C -= J.transpose() * fA;
}
}
}
}
if (tree->getNumPositionConstraints()) {
int nc = tree->getNumPositionConstraints();
const double alpha = 5.0; // 1/time constant of position constraint
// satisfaction (see my latex rigid body notes)
prog.AddContinuousVariables(
nc, "position constraint force"); // don't actually need to use the
// decision variable reference that
// would be returned...
// then compute the constraint force
auto phi = tree->positionConstraints(kinsol);
auto J = tree->positionConstraintsJacobian(kinsol, false);
auto Jdotv = tree->positionConstraintsJacDotTimesV(kinsol);
// phiddot = -2 alpha phidot - alpha^2 phi (0 + critically damped
// stabilization term)
prog.AddLinearEqualityConstraint(
J, -(Jdotv + 2 * alpha * J * v + alpha * alpha * phi), {vdot});
H_and_neg_JT.conservativeResize(NoChange, H_and_neg_JT.cols() + J.rows());
H_and_neg_JT.rightCols(J.rows()) = -J.transpose();
}
// add [H,-J^T]*[vdot;f] = -C
prog.AddLinearEqualityConstraint(H_and_neg_JT, -C);
prog.Solve();
// prog.PrintSolution();
StateVector<double> dot(nq + nv);
dot << kinsol.transformPositionDotMappingToVelocityMapping(
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Identity(
nq, nq)) *
v,
vdot.value();
return dot;
}
int main(int argc, char** argv) {
MatrixXf m(3,3);
m << 1,2,3,4,5,6,7,8,9;
Vector3f v = m.col(0);
Vector3f v2 = m.col(1);
Vector3f v3 = m.col(2);
cout<<"Mat = "<<m<<endl;
cout<<"vector = " << v<<endl;
MatrixXf n = (m.rowwise() -= v);
cout<<"Mat2 = "<<n<<endl;
Matrix3f r;
r.col(0) = v;
r.col(1) = v2;
r.col(2) = v3;
cout<<"Mat3 = "<<r<<endl;
r *= 2;
cout<<"Mat3 = "<<r<<endl;
cout<<"Mat3 = "<<m<<endl;
float arr[] = {1,2,3};
vector<float> w(arr, arr + sizeof(arr) / sizeof(float));
for(int i = 0; i < w.size(); i+=1)
cout<<w[i]<<"\t";
cout<<endl<<"---------------"<<endl;
/** Inverting a singular matrix. **/
m << 1,0,0,1,0,0,1,0,0;
cout<<" Should segfault/ get garbage: "<<m.determinant()<<endl;
/** Affine matrix in Eigen. */
MatrixXf ml(4,4);
ml << 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16;
Affine3f t;
Vector3f d(10,20,30);
t.linear() = ml.transpose().block(0,0,3,3);
t.translation() = ml.block(0,3,3,1);
cout<<"Affine: "<<endl<<t.affine()<<endl;
Vector3f tt(1,2,3);
t.translation() += tt;
cout<<"Affine after translation: "<<endl<<t.affine()<<endl;
/** Blocks. */
MatrixXf matr(3,2);
matr << 1,2,3,4,5,6;
MatrixXf combo(4,2);
combo <<matr, MatrixXf::Ones(1,matr.cols());
cout<<"Matr = "<<matr<<endl;
cout<<"Comb = "<<combo<<endl;
MatrixXf rrr = combo.block(0,0,3,2);
cout<<"rrr = "<<rrr<<endl;
/** Filling up a matrix*/
std::cout<<"---------------------------------------"<<std::endl;
MatrixXf matF(5,3);
Vector3f vF(1,.3,3);
Vector3f vF1(.123,.2,.3);
Vector3f vF2(33.4,23.3,12.07);
Vector3f vF3(0.54,8.96,14.3);
Vector3f vF4(8.9,0.34,32.2);
matF.row(0) = vF;
matF.row(1) = vF1;
matF.row(2) = vF2;
matF.row(3) = vF3;
matF.row(4) = vF4;
RowVector3f means = matF.colwise().sum()/5;
MatrixXf centered = (matF.rowwise() - means);
cout<<"Matrix Fill = \n"<<matF<<std::endl;
cout<<"Means = \n"<<means<<std::endl;
cout<<"Centered = \n"<<centered<<std::endl;
Eigen::JacobiSVD<MatrixXf> svd(centered / sqrt(5), ComputeFullV);
cout << "Its singular values are:" << endl << svd.singularValues() << endl;
cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;
MatrixXf V = svd.matrixV();
Vector3f f1 = V.col(0);
Vector3f f2 = V.col(1);
Vector3f f3 = V.col(2);
cout << "f1(0)" << f1(0) << endl;
VectorXf faa(V.rows());
faa.setZero();
//for (int i= 0; i < V.rows(); i++)
V.col(2) = faa;
cout << "V " << V<< endl;
cout << "<v1,v2>" << f1.dot(f2) << endl;
cout << "<v1,v3>" << f1.dot(f3) << endl;
cout << "<v3,v2>" << f3.dot(f2) << endl;
std::cout<<"---------------------------------------"<<std::endl;
Vector3f o3(1,2,3), o2(4,5,6);
cout << "outer? : "<< o3.cwiseProduct(o2) <<endl;
MatrixXd mxx(3,3);
mxx << 1,0,0,0,2,0,0,0,3;
cout << "mxx : "<< endl<<mxx <<endl;
vector<double> v31(3);
VectorXd::Map(&v31[0], 3) = mxx.row(0);
cout << "v: "<<endl;
for(int i=0; i < 3; i++)
cout<< v31[i]<< " " <<endl;
cout<<endl;
v31[1] = 100;
cout << "v: "<<endl;
for(int i=0; i < 3; i++)
cout << v31[i]<< " " <<endl;
cout<<endl;
cout << " mxx : "<<endl <<mxx<<endl;
Matrix3d tmat = Matrix3d::Identity();
tmat(0,0) = 0;
tmat(1,1) = 10;
tmat(1,2) = 20;
cout << "tmat: " << tmat<<endl;
cout << "------------\n";
for (unsigned i=0; i < tmat.rows(); i++) {
double sum = tmat.row(i).sum() + numeric_limits<double>::min();
tmat.row(i) /= sum;
}
cout << tmat << endl;
MatrixXd src(4,3);
src<< 0,0,0,1,1,1,2,2,2,3,3,3;
MatrixXd target(1,3);
target<< 1,1,1;
cout <<"cloud err: "<< (src.rowwise() -target.row(0)).rowwise().squaredNorm().transpose()<<endl;
return 0;
}