Matrix<double,7,1> Sim3
::log(const Sim3& sim3)
{
Vector7d res;
double scale = sim3.scale();
Vector3d t = sim3.translation_;
double theta;
Vector4d omega_sigma = ScSO3::logAndTheta(sim3.scso3_, &theta);
Vector3d omega = omega_sigma.head<3>();
double sigma = omega_sigma[3];
Matrix3d Omega = SO3::hat(omega);
Matrix3d W = calcW(theta, sigma, scale, Omega);
//Vector3d upsilon = W.jacobiSvd(ComputeFullU | ComputeFullV).solve(t);
Vector3d upsilon = W.partialPivLu().solve(t);
res.segment(0,3) = upsilon;
res.segment(3,3) = omega;
res[6] = sigma;
return res;
}
geometry_msgs::Pose MoveitPlanningInterface::convToPose(Vector3f plane_normal, Vector3f major_axis, Vector3f centroid) {
geometry_msgs::Pose pose;
Affine3d Affine_des_gripper;
Vector3d xvec_des,yvec_des,zvec_des,origin_des;
float temp;
temp = major_axis[0];
major_axis[0] = major_axis[1];
major_axis[1] = temp;
Vector3f rotation = major_axis;
major_axis[0] = (sqrt(3)/2)*rotation[0] - rotation[1]/2;
major_axis[1] = (sqrt(3)/2)*rotation[1] + rotation[0]/2;
Matrix3d Rmat;
for (int i=0;i<3;i++) {
origin_des[i] = centroid[i]; // convert to double precision
zvec_des[i] = -plane_normal[i]; //want tool z pointing OPPOSITE surface normal
xvec_des[i] = major_axis[i];
}
// origin_des[2]+=0.02; //raise up 2cm
yvec_des = zvec_des.cross(xvec_des); //construct consistent right-hand triad
Rmat.col(0)= xvec_des;
Rmat.col(1)= yvec_des;
Rmat.col(2)= zvec_des;
Affine_des_gripper.linear()=Rmat;
Affine_des_gripper.translation()=origin_des;
//convert des pose from Affine to geometry_msgs::PoseStamped
pose = transformEigenAffine3dToPose(Affine_des_gripper);
return pose;
}
TEST(Rotation, TestEulerAnglesAndMatrices) {
// Randomly generate euler angles, convert to a matrix, then convert back.
// Check that (1) the rotation matrix has det(R)==1, and (2), that we get the
// same angles back.
math::RandomGenerator rng(0);
for (int ii = 0; ii < 1000; ++ii) {
Vector3d e1;
e1.setRandom();
// Converting from rotation matrices to euler angles is only valid when phi,
// theta, and psi are all < 0.5*PI. Otherwise the problem has multiple
// solutions, and we can only return one of them with our function.
e1 *= 0.5 * M_PI;
Matrix3d R = EulerAnglesToMatrix(e1);
EXPECT_NEAR(1.0, R.determinant(), 1e-8);
Vector3d e2 = MatrixToEulerAngles(R);
EXPECT_NEAR(0.0, S1Distance(e1(0), e2(0)), 1e-8);
EXPECT_NEAR(0.0, S1Distance(e1(1), e2(1)), 1e-8);
EXPECT_NEAR(0.0, S1Distance(e1(2), e2(2)), 1e-8);
EXPECT_TRUE(R.isApprox(EulerAnglesToMatrix(e2), 1e-4));
}
}
Vector6d logmap_se3(Matrix4d T){
Matrix3d R, Id3 = Matrix3d::Identity();
Vector3d Vt, t, w;
Matrix3d V = Matrix3d::Identity(), w_hat = Matrix3d::Zero();
Vector6d x;
Vt << T(0,3), T(1,3), T(2,3);
w << 0.f, 0.f, 0.f;
R = T.block(0,0,3,3);
double cosine = (R.trace() - 1.f)/2.f;
if(cosine > 1.f)
cosine = 1.f;
else if (cosine < -1.f)
cosine = -1.f;
double sine = sqrt(1.0-cosine*cosine);
if(sine > 1.f)
sine = 1.f;
else if (sine < -1.f)
sine = -1.f;
double theta = acos(cosine);
if( theta > 0.000001 ){
w_hat = theta*(R-R.transpose())/(2.f*sine);
w = skewcoords(w_hat);
Matrix3d s;
s = skew(w) / theta;
V = Id3 + s * (1.f-cosine) / theta + s * s * (theta - sine) / theta;
}
t = V.inverse() * Vt;
x.head(3) = t;
x.tail(3) = w;
return x;
}
void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
Vector3d mstart(start[0], start[1], start[2]);
// convert from blender column major
Matrix3d mbasis = CreateMatrix(basis[0][0], basis[1][0], basis[2][0],
basis[0][1], basis[1][1], basis[2][1],
basis[0][2], basis[1][2], basis[2][2]);
Matrix3d mrest = CreateMatrix(rest[0][0], rest[1][0], rest[2][0],
rest[0][1], rest[1][1], rest[2][1],
rest[0][2], rest[1][2], rest[2][2]);
double mlength(length);
if (qseg->Composite()) {
Vector3d cstart(0, 0, 0);
Matrix3d cbasis;
cbasis.setIdentity();
qseg->SetTransform(mstart, mrest, mbasis, 0.0);
qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
}
else
qseg->SetTransform(mstart, mrest, mbasis, mlength);
}
Needle::Needle(const Vector3d& pos, const Matrix3d& rot, double degrees, double r, float c0, float c1, float c2, World* w, ThreadConstrained* t, int constrained_vertex_num)
: EnvObject(c0, c1, c2, NEEDLE)
, angle(degrees)
, radius(r)
, thread(t)
, constraint(constrained_vertex_num)
, world(w)
, position_offset(0.0, 0.0, 0.0)
, rotation_offset(Matrix3d::Identity())
{
assert(((thread == NULL) && (constrained_vertex_num == -1)) || ((thread != NULL) && (constrained_vertex_num != -1)));
updateConstraintIndex();
assert(degrees > MIN_ANGLE);
assert(r > MIN_RADIUS);
double arc_length = radius * angle * M_PI/180.0;
int pieces = ceil(arc_length/2.0);
for (int piece=0; piece<pieces; piece++) {
Intersection_Object* obj = new Intersection_Object();
obj->_radius = radius/8.0;
i_objs.push_back(obj);
}
if (thread != NULL) {
Matrix3d new_rot = AngleAxisd(-M_PI/2.0, rot.col(0)) * (AngleAxisd((angle) * M_PI/180.0, rot.col(1)) * rot);
setTransform(pos - radius * rot.col(1), new_rot);
} else {
setTransform(pos, rot);
}
}
/// Constructs a vector-valued attribute with the given name
XMLAttrib::XMLAttrib(const std::string& name, const Matrix3d& matrix_value)
{
this->processed = false;
this->name = name;
std::ostringstream oss;
// set the first value of the matrix
oss << str(matrix_value(0,0));
// for each row of the matrix
for (unsigned j=0; j< matrix_value.rows(); j++)
{
// determine column iteration
unsigned i = (j == 0) ? 1 : 0;
// for each column of the matrix
for (; i< matrix_value.columns(); i++)
oss << " " << str(matrix_value(j,i));
// separate rows with a semicolon
oss << ";";
}
// get the string value
this->value = oss.str();
}
double Distance( const Conic& c1, const Conic& c2, double circle_radius )
{
const Matrix3d Q = c1.Dual * c2.C;
// const double dsq = 3 - trace(Q)* pow(determinant(Q),-1.0/3.0);
const double dsq = 3 - Q.trace()* 1.0 / cbrt(Q.determinant());
return sqrt(dsq) * circle_radius;
}
MatrixXd Utils::calculateHomographyMatrix(vector<Vector3i> selectedPoints, vector<Vector3d> realWorldPoints)
{
MatrixXd A(8, 8);
MatrixXd B(8,1);
for (uint i = 0; i < realWorldPoints.size(); i++)
{
double realWorldX = realWorldPoints.at(i).x();
double realWorldY = realWorldPoints.at(i).y();
int selectedPointX = selectedPoints.at(i).x();
int selectedPointY = selectedPoints.at(i).y();
A.row(i*2) << realWorldX, realWorldY, 1, 0, 0, 0, -realWorldX*selectedPointX, -realWorldY*selectedPointX;
A.row(i*2 + 1) << 0, 0, 0, realWorldX, realWorldY, 1, -realWorldX*selectedPointY, -realWorldY*selectedPointY;
B.row(i*2) << selectedPointX;
B.row(i*2 +1) << selectedPointY;
}
//A = A.inverse().eval();
MatrixXd x = A.fullPivLu().solve(B);
Matrix3d H;
H << x(0), x(1), x(2), x(3), x(4), x(5), x(6), x(7), 1;
return H.inverse();
}
Vector6d gc_asd_to_av(Vector4d asd) {
Vector6d av;
Vector3d aa = asd.head(3);
// double d_inv = asd(3);
// double sig_d_inv = (1.0 - exp(-asd(3))) / (2.0 * (1.0 + exp(-asd(3))));
// double sig_d_inv = -log(1.0/asd(3) - 1.0);
// double sig_d_inv = log( (2.0 * asd(3) + 1.0) / (1.0 - 2.0*asd(3)) );
// double sig_d_inv = atan(asd(3)) / 2.0;
// double sig_d_inv = atan2(asd(3), 1.0) / 2.0;
// double sig_d_inv = atan2(asd(3), 1.0);
// double sig_d_inv = atan2(asd(3), 1.0) * 1.0;
// double sig_d_inv = tan(4.0 * asd(3));
double sig_d_inv = log(asd(3));
// cout << "sig_d_inv = " << sig_d_inv << endl;
// double sig_d_inv = cos(asd(3)) / sin(asd(3));
// double sig_d_inv = sin(asd(3)) / cos(asd(3));
// double sig_d_inv = sin(asd(3)) / cos(asd(3));
Matrix3d R = gc_Rodriguez(aa);
// av.head(3) = R.col(2) / sig_d_inv;
av.head(3) = R.col(2) * sig_d_inv;
av.tail(3) = R.col(0);
return av;
}
void SimObject::rotateAroundAxisDnD(double angle, Vector3d axis)
{
Matrix3d m;
axis.normalize();
m.setRotationAroundAxis(axis, angle);
rotateDnD(m, position);
}
Vector6d gc_aid_to_av(Vector4d aid) {
Vector6d av;
Vector3d aa = aid.head(3);
double d = 1.0 / aid(3);
Matrix3d R = gc_Rodriguez(aa);
av.head(3) = R.col(2) * d;
av.tail(3) = R.col(0);
// Vector6d av;
// double a = aid[0];
// double b = aid[1];
// double g = aid[2];
// double t = aid[3];
//
// double s1 = sin(a);
// double c1 = cos(a);
// double s2 = sin(b);
// double c2 = cos(b);
// double s3 = sin(g);
// double c3 = cos(g);
//
// Matrix3d R;
// R <<
// c2 * c3, s1 * s2 * c3 - c1 * s3, c1 * s2 * c3 + s1 * s3,
// c2 * s3, s1 * s2 * s3 + c1 * c3, c1 * s2 * s3 - s1 * c3,
// -s2, s1 * c2, c1 * c2;
//
// double d = 1.0 / t;
// av.head(3) = -R.col(2) * d;
// av.tail(3) = R.col(1);
return av;
}
void Angle::fromMatrix(const Matrix3d& matrix)
{
assert(matrix.x() == 3 && matrix.y() == 3);
double forward[3];
double left[3];
double up[3];
forward[0] = matrix[0][0];
forward[1] = matrix[1][0];
forward[2] = matrix[2][0];
left[0] = matrix[0][1];
left[1] = matrix[1][1];
left[2] = matrix[2][1];
up[2] = matrix[2][2];
double xyDist = sqrt(forward[0] * forward[0] + forward[1] * forward[1]);
if (xyDist > 0.0001)
{
(*this)[PITCH] = RAD2DEG( atan2( -forward[2], xyDist ) );
(*this)[YAW] = RAD2DEG( atan2( forward[1], forward[0] ) );
(*this)[ROLL] = RAD2DEG( atan2( left[2], up[2] ) );
}
else
{ // gimbal lock
(*this)[PITCH] = RAD2DEG( atan2( -forward[2], xyDist ) );
(*this)[YAW] = RAD2DEG( atan2( -left[0], left[1] ) );
(*this)[ROLL] = 0;
}
}
double distPtTri(Vector3d p, Matrix4d m){
/// compute distance between a triangle and a point
double s[4];
Vector3d a,b,c,n;
a << m(0,0), m(0,1), m(0,2);
b << m(1,0), m(1,1), m(1,2);
c << m(2,0), m(2,1), m(2,2);
n << m(3,0), m(3,1), m(3,2);
double k=(n-a).dot(a-p);
if(k<0) return HUGE_VALF;
s[0]=distPtLin(p,a,b);
s[1]=distPtLin(p,b,c);
s[2]=distPtLin(p,c,a);
Matrix3d A;
A << b(0)-a(0), c(0)-a(0), n(0)-a(0),
b(1)-a(1), c(1)-a(1), n(1)-a(1),
b(2)-a(2), c(2)-a(2), n(2)-a(2);
Vector3d v = A.inverse()*(p-a);
if(v(0)>0 && v(1)>0 && v(0)+v(1)<1){
s[3]=k*k;
}else{
s[3] = HUGE_VALF;
}
return min(min(min(s[0],s[1]),s[2]),s[3]);
}
/**
* @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);
}
}
}
bool ContainerUtility::checkRange(Matrix3d & mm, double minVal, double maxVal)
{
assert(minVal < maxVal);
// Go through each element in the matrix and ensure they are not
// NaN and fall within the specified min and max values.
for (int ii = 0; ii < mm.rows(); ii++)
{
for (int jj = 0; jj < mm.cols(); jj++)
{
if (isnan(mm(ii, jj)))
{
CONTROLIT_WARN << "(Matrix): NaN detected at index (" << ii << ", " << jj << "), returning false!";
return false;
}
if (mm(ii, jj) > maxVal || mm(ii, jj) < minVal)
{
CONTROLIT_WARN << "(Matrix): Value of out range at index (" << ii << ", " << jj << "), returning false.\n"
" - value = " << mm(ii, jj) << "\n"
" - minVal = " << minVal << "\n"
" - maxVal = " << maxVal;
return false;
}
}
}
return true;
}
Matrix4d TrfmRotateExpMap::getSecondDeriv(const Dof *q1, const Dof *q2){
Vector3d q(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());
// derivative wrt which mDofs
int j=-1, k=-1;
for(unsigned int i=0; i<mDofs.size(); i++) {
if(q1==mDofs[i]) j=i;
if(q2==mDofs[i]) k=i;
}
assert(j!=-1);
assert(k!=-1);
assert(j>=0 && j<=2);
assert(k>=0 && k<=2);
Matrix3d R = utils::rotation::expMapRot(q);
Matrix3d J = utils::rotation::expMapJac(q);
Matrix3d Jjss = utils::makeSkewSymmetric(J.col(j));
Matrix3d Jkss = utils::makeSkewSymmetric(J.col(k));
Matrix3d dJjdkss = utils::makeSkewSymmetric(utils::rotation::expMapJacDeriv(q, k).col(j));
Matrix3d d2Rdidj = (Jjss*Jkss + dJjdkss)*R;
Matrix4d d2Rdidj4 = Matrix4d::Zero();
d2Rdidj4.topLeftCorner(3,3) = d2Rdidj;
return d2Rdidj4;
}
Matrix3d rotateXYZ(const double &alpha, const double &beta, const double &gamma)
{
Matrix3d mat;
mat.setOnes(3,3);
const double cosa = cos(alpha);
const double sina = sin(alpha);
const double cosb = cos(beta);
const double sinb = sin(beta);
const double cosg = cos(gamma);
const double sing = sin(gamma);
mat(0, 0) = cosb * cosg;
mat(0, 1) = -sing * cosb;
mat(0, 2) = sinb;
mat(1, 0) = sina * sinb * cosg + cosa * sing;
mat(1, 1) = - sina * sinb *sing + cosa * cosg;
mat(1, 2) = - sina * cosb;
mat(2, 0) = - cosa * sinb * cosg + sina * sing;
mat(2, 1) = sina * sinb * sing + sina *cosg;
mat(2, 2) = cosa * cosb;
// std::cout.precision(5);
// std::cout.setf(std::ios::fixed, std::ios::floatfield);
// std::cout << "matrix is:\n\n" << mat << "\n" << std::endl;
return mat;
}
void Point::optimize(const size_t n_iter)
{
Vector3d old_point = pos_;
double chi2 = 0.0;
Matrix3d A;
Vector3d b;
for(size_t i=0; i<n_iter; i++)
{
A.setZero();
b.setZero();
double new_chi2 = 0.0;
// compute residuals
for(auto it=obs_.begin(); it!=obs_.end(); ++it)
{
Matrix23d J;
const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotationMatrix(), J);
const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
new_chi2 += e.squaredNorm();
A.noalias() += J.transpose() * J;
b.noalias() -= J.transpose() * e;
}
// solve linear system
const Vector3d dp(A.ldlt().solve(b));
// check if error increased
if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0]))
{
#ifdef POINT_OPTIMIZER_DEBUG
cout << "it " << i
<< "\t FAILURE \t new_chi2 = " << new_chi2 << endl;
#endif
pos_ = old_point; // roll-back
break;
}
// update the model
Vector3d new_point = pos_ + dp;
old_point = pos_;
pos_ = new_point;
chi2 = new_chi2;
#ifdef POINT_OPTIMIZER_DEBUG
cout << "it " << i
<< "\t Success \t new_chi2 = " << new_chi2
<< "\t norm(b) = " << vk::norm_max(b)
<< endl;
#endif
// stop when converged
if(vk::norm_max(dp) <= EPS)
break;
}
#ifdef POINT_OPTIMIZER_DEBUG
cout << endl;
#endif
}
int main(int, char**)
{
cout.precision(3);
Matrix3d m = Matrix3d::Random();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Its inverse is:" << endl << m.inverse() << endl;
return 0;
}
Vector6d Line3D::toCartesian() const{
Vector6d cartesian;
cartesian.tail<3>() = d()/d().norm();
Matrix3d W=-_skew(d());
double damping = 1e-9;
Matrix3d A = W.transpose()*W+(Matrix3d::Identity()*damping);
cartesian.head<3>() = A.ldlt().solve(W.transpose()*w());
return cartesian;
}
Vector4d gc_av_to_asd(Vector6d av) {
Vector4d asd;
Vector3d a = av.head(3);
Vector3d x = av.tail(3); // v
Vector3d y = a.cross(x); // n
// Vector2d w(y.norm(), x.norm());
// w /= w.norm();
// asd(3) = asin(w(1));
double depth = x.norm() / y.norm();
// double sig_d = log( (2.0*depth + 1.0) / (1.0 - 2.0*depth));
// double quotient = depth / 0.5;
// int integer_quotient = (int)quotient;
// double floating_quotient = quotient - (double)integer_quotient;
// depth = depth * floating_quotient;
// double sig_d = log(2.0*depth + 1.0) - log(1.0 - 2.0*depth);
// double sig_d = atan(1.0 / (1.0*depth) );
// double sig_d = atan(depth);
// double sig_d = atan2(1.0, depth);
// double sig_d = atan(depth);
// double sig_d = atan2(1.0, depth) / 4.0;
double sig_d = 1.0 / exp(-depth);
asd(3) = sig_d;
// cout << "sig_d = " << sig_d << endl;
// asd(3) = depth;
// double sig_d = tan(1.0/depth);
// double sig_d = tan(2.0/depth);
// double sig_d = tan(2.0*depth);
// double sig_d = (1.0 - exp(-1.0/depth)) / (2.0*(1.0 + exp(-1.0/depth)));
// double sig_d = (1.0 - exp(-depth)) / (2.0*(1.0 + exp(-depth)));
// double sig_d = 1.0 / (1.0 + exp(-1.0/depth));
// double sig_d = 1.0 / (1.0 + exp(-depth));
x /= x.norm();
y /= y.norm();
Vector3d z = x.cross(y);
Matrix3d R;
R.col(0) = x;
R.col(1) = y;
R.col(2) = z;
Vector3d aa = gc_Rodriguez(R);
asd(0) = aa(0);
asd(1) = aa(1);
asd(2) = aa(2);
return asd;
}
int main(int, char**)
{
cout.precision(3);
Matrix3d m = Matrix3d::Random();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Here is the sum of each row:" << endl << m.rowwise().sum() << endl;
return 0;
}
static bool isNumericallySound(const Matrix3d& E)
{
double Emax = E.cwiseAbs().sum();
double Esum = E.cwiseAbs().maxCoeff();
if (Esum < 1E-10) return false;
Esum = Esum / Emax; // Eigen: Ecand.cwiseAbs().sum() / Ecand.lpNorm<Infinity>()
return !std::isnan(Esum) && !std::isinf(Esum);
}
int main(int, char**)
{
cout.precision(3);
Matrix3d m = Matrix3d::Identity();
m.col(1) = Vector3d(4,5,6);
cout << m << endl;
return 0;
}
int main(int, char**)
{
cout.precision(3);
Matrix3d m = Matrix3d::Random();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Here is the minimum of each column:" << endl << m.colwise().minCoeff() << endl;
return 0;
}
Vector3d Compound::PointOfReference::vectorFromPointAndNearVector(Compound::PointPtr point, Vector3d vector, int i) {
if(i > 10 | vector.squaredNorm() > 10000 | vector != vector) {
//std::cout << "Compound.cpp vector:\n" << vector << std::endl;
//return vector;
return Vector3d(0,0,0);
}
Vector3d v1 = point->getPosition()->getVector();
//Vector3d v0 = point->getPosition()->getVector();
//assert(v0 == v0);
double epsilon = 0.00001;
Manifold* space = point->getPosition()->getSpace();
//std::cout << "Compound.cpp i:\t" << i << std::endl;
Vector3d v0 = this->pointFromVector(vector)->getVector(space);
Vector3d vx = this->pointFromVector(vector + Vector3d(epsilon,0,0))->getVector(space);
Vector3d vy = this->pointFromVector(vector + Vector3d(0,epsilon,0))->getVector(space);
Vector3d vz = this->pointFromVector(vector + Vector3d(0,0,epsilon))->getVector(space);
assert(vz == vz);
Matrix3d jacobean;
jacobean << vx-v0,vy-v0,vz-v0;
jacobean /= epsilon;
/*std::cout << "getPosition()->getVector():\n" << getPosition()->getVector() << std::endl;
std::cout << "vector:\n" << vector << std::endl;
std::cout << "v0:\n" << v0 << std::endl;
std::cout << "vx:\n" << vx << std::endl;
std::cout << "vy:\n" << vy << std::endl;
std::cout << "vz:\n" << vz << std::endl;*/
//std::cout << "jacobean:\n" << jacobean << std::endl;
Vector3d delta = jacobean.inverse()*(v1-v0);
//std::cout << "v1-v0:\n" << v1-v0 << std::endl;
//std::cout << "delta:\n" << delta << std::endl;
if(delta != delta) {
return Vector3d(0,0,0);
}
//std::cout << "delta.norm():\t" << delta.norm() << std::endl;
double squaredNorm = delta.squaredNorm();
double max = 5;
if(squaredNorm > max*max) {
delta = max*delta/sqrt(squaredNorm);
}
if(squaredNorm < EPSILON*EPSILON) {
/*std::cout << "v1-v0:\n" << v1-v0 << std::endl;
std::cout << "delta:\n" << delta << std::endl;
std::cout << "vector:\n" << vector << std::endl;
std::cout << "delta+vector:\n" << delta+vector << std::endl;
std::cout << "pointOfReference->vectorFromPoint(point):\n" << pointOfReference->getGeodesic(point->getPosition())->getVector() << std::endl;*/
assert(pointFromVector(delta+vector)->getPosition()->getSpace() == point->getPosition()->getSpace());
assert((pointFromVector(delta+vector)->getPosition()->getVector() - point->getPosition()->getVector()).squaredNorm() < EPSILON);
/*assert((pointOfReference->getSpace()->pointFromVector(pointOfReference, vector)->getVector() - point->getPosition()->getVector()).squaredNorm() < EPSILON);
assert((this->pointFromVector(delta+vector)->getPosition()->getVector() - pointOfReference->getSpace()->pointFromVector(pointOfReference, vector)->getVector()).squaredNorm() < EPSILON);
assert((delta+vector - pointOfReference->getSpace()->vectorFromPoint(pointOfReference, point->getPosition())).squaredNorm() < EPSILON);*/ //Only for portals that connect to themselves.
//std::cout << "i:\t" << i << std::endl;
return delta+vector;
} else {
return vectorFromPointAndNearVector(point, delta+vector, i+1);
}
}
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 BodyNode::updateTransform() {
mT = mJointParent->getLocalTransform();
if (mNodeParent) mW = mNodeParent->mW * mT;
else mW = mT;
// update the inertia matrix
Matrix3d R = mW.topLeftCorner(3,3);
if(mVizShape!=NULL)
mIc = R*mI*R.transpose();
}
RigidBodyInertia operator*(const Rotation& M,const RigidBodyInertia& I){
//mb=ma
//hb=R*h
//Ib = R(Ia)R' with r=0
Matrix3d R = Map<Matrix3d>(M.data);
RotationalInertia Ib;
Map<Matrix3d>(Ib.data) = R.transpose()*(Map<Matrix3d>(I.I.data)*R);
return RigidBodyInertia(I.m,M*I.h,Ib,mhi);
}
