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 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;
}
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]);
}
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;
}
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);
}
}
vector<pair<int, Vector3d>> DataGenerator::getImage()
{
vector<pair<int, Vector3d>> image;
Vector3d position = getPosition();
Matrix3d quat = getRotation(); //R_gb
printf("max: %d\n", current_id);
for (int i = 0; i < NUM_POINTS; i++)
{
double xx = pts[i * 3 + 0] - position(0);
double yy = pts[i * 3 + 1] - position(1);
double zz = pts[i * 3 + 2] - position(2);
Vector3d local_point = Ric.inverse() * (quat.inverse() * Vector3d(xx, yy, zz) - Tic);
xx = local_point(0);
yy = local_point(1);
zz = local_point(2);
if (std::fabs(atan2(xx, zz)) <= PI * FOV / 2 / 180
&& std::fabs(atan2(yy, zz)) <= PI * FOV / 2 / 180
&& zz > 0)
{
int n_id = before_feature_id.find(i) == before_feature_id.end() ?
current_id++ : before_feature_id[i];
//#if WITH_NOISE
// Vector3d disturb = Vector3d(distribution(generator) * sqrt(pts_cov(0, 0)) / zz / zz,
// distribution(generator) * sqrt(pts_cov(1, 1)) / zz / zz,
// 0
// );
// image.push_back(make_pair(n_id, disturb + Vector3d(xx / zz, yy / zz, 1)));
//#else
image.push_back(make_pair(n_id, Vector3d(xx, yy, zz)));
printf ("id %d, (%d %d %d)\n", n_id,
pts[i * 3 + 0] ,
pts[i * 3 + 1] ,
pts[i * 3 + 2]
);
//#endif
current_feature_id[i] = n_id;
}
}
before_feature_id = current_feature_id;
current_feature_id.clear();
//sort(image.begin(), image.end(), [](const pair<int, Vector3d> &a,
// const pair<int, Vector3d> &b)
//{
// return a.first < b.first;
//});
return image;
}
/**
*@brief 手先速度から関節角速度を取得
* @param v 手先速度
* @return 関節角速度
*/
Vector3d RobotArm::calcJointVel(Vector3d v)
{
Matrix3d J = calcJacobian(theta);
Matrix3d Jinv = J.inverse();
Vector3d vf(v(0), v(1), v(2));
Vector3d A = Jinv * vf;
//std::cout << Jinv << std::endl << std::endl;
return A;
}
double Tetrahedra<ConcreteShape>::estimatedElementRadius() {
Matrix3d invJ;
double detJ;
std::tie(invJ, detJ) = ConcreteShape::inverseJacobian(mVtxCrd);
Matrix3d J = invJ.inverse();
VectorXcd eivals = J.eigenvalues();
// get minimum h (smallest direction)
Vector3d eivals_norm;
for(int i=0;i<3;i++) {
eivals_norm(i) = std::norm(eivals[i]);
}
return eivals_norm.minCoeff();
}
/**
* input needs at least 2 correspondences of non-parallel lines
* the resulting R and t works as below: x'=Rx+t for point pair(x,x');
* @param vLineA
* @param vLineB
* @param R
* @param t
*/
void
LineExtract::ComputeRelativeMotion_svd(vector<Line3d> vLineA, vector<Line3d> vLineB, Matrix3d &R, Vector3d &t) {
if (vLineA.size() < 2) {
cerr << "Error in computeRelativeMotion_svd: input needs at least 2 pairs!\n";
return;
}
// convert to the representation of Zhang's paper
for (int i = 0; i < vLineA.size(); ++i) {
Vector3d l, m;
if (vLineA[i].u.norm() < 0.9) {
l = vLineA[i].EndB - vLineA[i].EndA;
m = (vLineA[i].EndA + vLineA[i].EndB) * 0.5;
vLineA[i].u = l / l.norm();
vLineA[i].d = vLineA[i].u.cross(m);
// cout<<"in computeRelativeMotion_svd compute \n";
}
if (vLineB[i].u.norm() < 0.9) {
l = vLineB[i].EndB - vLineB[i].EndA;
m = (vLineB[i].EndA + vLineB[i].EndB) * 0.5;
vLineB[i].u = l * (1 / l.norm());
vLineB[i].d = vLineB[i].u.cross(m);
}
}
Matrix4d A = Matrix4d::Zero();
for (int i = 0; i < vLineA.size(); ++i) {
Matrix4d Ai = Matrix4d::Zero();
Ai.block<1, 3>(0, 1) = vLineA[i].u - vLineB[i].u;
Ai.block<3, 1>(1, 0) = vLineB[i].u - vLineA[i].u;
Ai.bottomRightCorner<3, 3>(1, 1) = SO3d::hat((vLineA[i].u + vLineB[i].u)).matrix();
A = A + Ai.transpose() * Ai;
}
Eigen::JacobiSVD<Matrix4d> svd(A, Eigen::ComputeFullV | Eigen::ComputeFullV);
Vector4d q = svd.matrixU().col(3);
R = Eigen::Quaterniond(q).matrix();
Matrix3d uu = Matrix3d::Zero();
Vector3d udr = Vector3d::Zero();
for (int i = 0; i < vLineA.size(); ++i) {
uu = uu + SO3d::hat(vLineB[i].u) * SO3d::hat(vLineB[i].u).matrix().transpose();
udr = udr + SO3d::hat(vLineB[i].u).transpose() * (vLineB[i].d - R * vLineA[i].d);
}
t = uu.inverse() * udr;
}
Vector3d DataGenerator::getAngularVelocity()
{
const double delta_t = 0.00001;
Matrix3d rot = getRotation();
t += delta_t;
Matrix3d drot = (getRotation() - rot) / delta_t;
t -= delta_t;
Matrix3d skew = rot.inverse() * drot;
#ifdef WITH_NOISE
Vector3d disturb = Vector3d(distribution(generator) * sqrt(gyr_cov(0, 0)),
distribution(generator) * sqrt(gyr_cov(1, 1)),
distribution(generator) * sqrt(gyr_cov(2, 2))
);
return disturb + Vector3d(skew(2, 1), -skew(2, 0), skew(1, 0));
#else
return Vector3d(skew(2, 1), -skew(2, 0), skew(1, 0));
#endif
}
//NOTE: this must be done after all parameters are computed
Vector4d CProxyCamera::getOpticalCenter(void) const
{
// orthographic case
Vector4d ans;
if (m_KR(2,0) == 0.0 && m_KR(2,1) == 0.0 &&
m_KR(2,2) == 0.0)
{
Vector3d vtmp[2];
for (int i = 0; i < 2; ++i)
for (int y = 0; y < 3; ++y)
{
vtmp[i][y] = m_KR(i,y);
}
Vector3d vtmp2 = vtmp[0].cross(vtmp[1]);
vtmp2.normalize();
for (int y = 0; y < 3; ++y)
{
ans[y] = vtmp2[y];
}
ans[3] = 0.0;
}
else
{
Matrix3d A;
Vector3d b;
for (int y = 0; y < 3; ++y)
{
for (int x = 0; x < 3; ++x)
{
A(y,x) = m_KR(y,x);
}
b[y] = - m_KT[y];
}
Matrix3d iA = A.inverse();
b = iA * b;
for (int y = 0; y < 3; ++y)
{
ans[y] = b[y];
}
ans[3] = 1.0;
}
return ans;
}
JNIEXPORT jobject JNICALL Java_com_mousebird_maply_Matrix3d_inverse
(JNIEnv *env, jobject obj)
{
try
{
Matrix3dClassInfo *classInfo = Matrix3dClassInfo::getClassInfo();
Matrix3d *inst = classInfo->getObject(env,obj);
if (!inst)
return NULL;
Matrix3d matInv = inst->inverse();
return MakeMatrix3d(env,matInv);
}
catch (...)
{
__android_log_print(ANDROID_LOG_VERBOSE, "Maply", "Crash in Matrix3d::inverse()");
}
return NULL;
}
MoveTo_TaskSpace::MoveTo_TaskSpace(const Affine3d &pose, const Vector3d &dp, double pos_tol,
const Vector3d &rpy_minus, const Vector3d &rpy_plus)
{
double X, Y, Z, roll, pitch, yaw ;
poseToXYZRPY(pose, X, Y, Z, roll, pitch, yaw) ;
c0 = Vector3d(X, Y, Z) ;
a0 = Vector3d(roll, pitch, yaw) ;
Vector3d c1 = c0 + dp ;
// We define a coordinate system with the Z axis pointing towards the target point
Matrix3d r = getLookAtMatrix(dp) ;
frame = r ;
iframe = r.inverse() ;
// we use a cylinder parameterization of the position
lower[0] = 0.0 ; upper[0] = dp.norm() ; // cylinder length
lower[1] = -M_PI ; upper[1] = M_PI ; // polar angle
lower[2] = 0.0 ; upper[2] = pos_tol ; // radius
const double small_ = 0.001 ;
double roll_min = std::max(a0.x() - fabs(rpy_minus.x()) - small_, -M_PI) ;
double roll_max = std::min(a0.x() + fabs(rpy_plus.x()) + small_, M_PI) ;
double pitch_min = std::max(a0.y() - fabs(rpy_minus.y()) - small_, -M_PI) ;
double pitch_max = std::min(a0.y() + fabs(rpy_plus.y()) + small_, M_PI) ;
double yaw_min = std::max(a0.z() - fabs(rpy_minus.z()) - small_, -M_PI) ;
double yaw_max = std::min(a0.z() + fabs(rpy_plus.z()) + small_, M_PI) ;
upper[3] = roll_max ; lower[3] = roll_min ;
upper[4] = pitch_max ; lower[4] = pitch_min ;
upper[5] = yaw_max ; lower[5] = yaw_min ;
}
MatrixXd der_logarithm_map(Matrix4d T)
{
MatrixXd dlogT_dT = MatrixXd::Zero(6,12);
// Approximate derivative of the logarithm_map wrt the transformation matrix
Matrix3d L1 = Matrix3d::Zero();
Matrix3d L2 = Matrix3d::Zero();
Matrix3d L3 = Matrix3d::Zero();
Matrix3d Vinv = Matrix3d::Identity();
Vector6d x = logmap_se3(T);
// estimates the cosine, sine, and theta
double b;
double cos_ = 0.5 * (T.block(0,0,3,3).trace() - 1.0 );
if(cos_ > 1.f)
cos_ = 1.f;
else if (cos_ < -1.f)
cos_ = -1.f;
double theta = acos(cos_);
double theta2 = theta*theta;
double sin_ = sin(theta);
double cot_ = 1.0 / tan( 0.5*theta );
double csc2_ = pow( 1.0/sin(0.5*theta) ,2);
// if the angle is small...
if( cos_ > 0.9999 )
{
b = 0.5;
L1(1,2) = -b;
L1(2,1) = b;
L2(0,2) = b;
L2(2,0) = -b;
L3(0,1) = -b;
L3(1,0) = b;
// form the full derivative
dlogT_dT.block(3,0,3,3) = L1;
dlogT_dT.block(3,3,3,3) = L2;
dlogT_dT.block(3,6,3,3) = L3;
dlogT_dT.block(0,9,3,3) = Vinv;
}
// if not...
else
{
// rotation part
double k;
Vector3d a;
a(0) = T(2,1) - T(1,2);
a(1) = T(0,2) - T(2,0);
a(2) = T(1,0) - T(0,1);
k = ( theta * cos_ - sin_ ) / ( 4 * pow(sin_,3) );
a = k * a;
L1.block(0,0,3,1) = a;
L2.block(0,1,3,1) = a;
L3.block(0,2,3,1) = a;
// translation part
Matrix3d w_skew = skew( x.tail(3) );
Vinv += w_skew * (1.f-cos_) / theta2 + w_skew * w_skew * (theta - sin_) / pow(theta,3);
Vinv = Vinv.inverse().eval();
// dVinv_dR
Vector3d t;
Matrix3d B, skew_t;
MatrixXd dVinv_dR(3,9);
t = T.block(0,3,3,1);
skew_t = skew( t );
// - form a
a = (theta*cos_-sin_)/(8.0*pow(sin_,3)) * w_skew * t
+ ( (theta*sin_-theta2*cos_)*(0.5*theta*cot_-1.0) - theta*sin_*(0.25*theta*cot_+0.125*theta2*csc2_-1.0))/(4.0*theta2*pow(sin_,4)) * w_skew * w_skew * t;
// - form B
Vector3d w;
Matrix3d dw_dR;
w = x.tail(3);
dw_dR.row(0) << -w(1)*t(1)-w(2)*t(2), 2.0*w(1)*t(0)-w(0)*t(1), 2.0*w(2)*t(0)-w(0)*t(2);
dw_dR.row(1) << -w(1)*t(0)+2.0*w(0)*t(1), -w(0)*t(0)-w(2)*t(2), 2.0*w(2)*t(1)-w(1)*t(2);
dw_dR.row(2) << -w(2)*t(0)+2.0*w(0)*t(2), -w(2)*t(1)+2.0*w(1)*t(2), -w(0)*t(0)-w(1)*t(1);
B = -0.5*theta*skew_t/sin_ - (theta*cot_-2.0)*dw_dR/(8.0*pow(sin_,2));
// - form dVinv_dR
dVinv_dR.col(0) = a;
dVinv_dR.col(1) = -B.col(2);
dVinv_dR.col(2) = B.col(1);
dVinv_dR.col(3) = B.col(2);
dVinv_dR.col(4) = a;
dVinv_dR.col(5) = -B.col(0);
dVinv_dR.col(6) = -B.col(1);
dVinv_dR.col(7) = B.col(0);
dVinv_dR.col(8) = a;
// form the full derivative
dlogT_dT.block(3,0,3,3) = L1;
dlogT_dT.block(3,3,3,3) = L2;
dlogT_dT.block(3,6,3,3) = L3;
dlogT_dT.block(0,9,3,3) = Vinv;
dlogT_dT.block(0,0,3,9) = dVinv_dR;
}
return dlogT_dT;
}
VectorXd rigidBodyDynamics::f(VectorXd x) {
Vector3d dr, dv, da, dw;
Matrix<double,12,12> lambda, dLambda;
VectorXd vec_dLambda;
int vecsize;
if (covProp) {
vecsize = 90;
} else {
vecsize = 12;
}
VectorXd dx(vecsize);
Vector3d r = x.segment<3>(0);
Vector3d v = x.segment<3>(3);
Vector3d a = x.segment<3>(6);
Vector3d w = x.segment<3>(9);
MatrixXd Bw = getBw();
Matrix3d J = _ir.getJ();
//Nonlinear State Model \dot x = f(x)
/*
* \mathbf{\dot r} = \mathbf{v}
*/
dr = v;
/*
* \mathbf{\dot v} = 0
*/
dv = Vector3d::Zero();
/*
* \frac{d \mathbf{a}_p}{dt} =
* \frac{1}{2}\left(\mathbf{[\omega \times]} +
* \mathbf{\omega} \cdot \mathbf{\bar q} \right) \mathbf{a}_p +
* \frac{2 q_4}{1+q_4} \mathbf{\omega}
*/
double c1, c2, c3;
c1 = 0.5;
c2 = 0.125 * w.dot(a); //NEW simplification
c3 = 1 - a.dot(a)/16;
da = -c1 * w.cross(a) + c2* a + c3 * w;
/*
* \dot \mathbf{w} = -\mathbf{J}^{-1} \mathbf{\omega} \times \mathbf{J} \mathbf{\omega}
*/
dw = - J.inverse() * w.cross(J * w);
if (covProp) {
//Covariance Propagation according to Lyapunov function
//see Brown & Hwang pg 204
//Compute Linear transition matrix
Matrix<double,12,12> A = Matrix<double,12,12>::Zero();
//position derivative
A.block<3,3>(0,3) = Matrix<double,3,3>::Identity();
//mrp kinematics
A.block<3,3>(6,6) = -0.5*crossProductMat(w) + w.dot(a)/8 * Matrix3d::Identity();
A.block<3,3>(6,9) = (1-a.dot(a/16))*Matrix3d::Identity();
//angular velocity dynamics
A.block<3,3>(9,9) = - J.inverse() * crossProductMat(w) * J;
lambda = vec2symmMat(x.segment<78>(12));
dLambda = A * lambda + lambda *A.transpose() + Bw * _Q * Bw.transpose();
vec_dLambda = symmMat2Vec(dLambda);
}
//write to dx
dx.segment<3>(0) = dr;
dx.segment<3>(3) = dv;
dx.segment<3>(6) = da;
dx.segment<3>(9) = dw;
if(covProp){
dx.segment<78>(12) = vec_dLambda;
}
return dx;
}
void Graph::solve(unsigned int iterations){
ROS_INFO(":: SOLVING! ::");
//Setup solver
g2o::SparseOptimizer sparseOptimizer;
SlamLinearSolver* linearSolver = new SlamLinearSolver();
linearSolver->setBlockOrdering(false);
SlamBlockSolver* blockSolver = new SlamBlockSolver(linearSolver);
g2o::OptimizationAlgorithmGaussNewton* solverGauss =
new g2o::OptimizationAlgorithmGaussNewton(blockSolver);
sparseOptimizer.setAlgorithm(solverGauss);
//Convert pose nodes to g2o node structure and add in the graph.
for(unsigned int i = 0; i < node_list.size(); i ++){
Node* curNode = node_list[i];
// ROS_INFO("Curnode id: %d", curNode->id);
// Convert the node.
GraphPose graph_pose = curNode->graph_pose;
g2o::SE2 converted_pose(graph_pose.x, graph_pose.y, graph_pose.theta);
// Create the vertex to put in the graph.
g2o::VertexSE2* vertex = new g2o::VertexSE2;
vertex->setId(curNode->id);
// ROS_INFO("Converted node id: %d", vertex->id());
vertex->setEstimate(converted_pose);
// Add to the graph
sparseOptimizer.addVertex(vertex);
}
// Set one pose fixed, to reduce complexity. This pose wont be changed by the optimizer.
sparseOptimizer.vertex(1)->setFixed(true);
// Convert the edges to g2o edges and add them in the graph
for(unsigned int i = 0; i < edge_list.size(); i++) {
// ROS_INFO("Adding edge: %d", i);
Edge* edge = edge_list[i];
//ROS_INFO("Edge parent id: %d, child id: %d", edge->parent_id, edge->child_id);
//Actually make the edge for the optimizer.
g2o::EdgeSE2* graph_edge = new g2o::EdgeSE2;
graph_edge->vertices()[0] = sparseOptimizer.vertex(edge->parent_id);
graph_edge->vertices()[1] = sparseOptimizer.vertex(edge->child_id);
//
g2o::SE2 se_mean(edge->mean[0], edge->mean[1], edge->mean[2]);
graph_edge->setMeasurement(se_mean);
Matrix3d cov;
cov = MatrixXd::Zero(3,3);
for(unsigned int i = 0; i < 3; i++) {
for(unsigned int j = 0; j < 3; j++) {
cov(i, j) = edge->covariance[i][j];
// ROS_INFO("Covariance[%d]: %f", i+j, covariance[i][j]);
}
}
graph_edge->setInformation(cov.inverse());
//Add edge to optimizer
sparseOptimizer.addEdge(graph_edge);
}
//Optimize!
sparseOptimizer.setVerbose(false);
sparseOptimizer.initializeOptimization();
sparseOptimizer.optimize(iterations);
//Convert the solved poses back
for(unsigned int i = 0; i < node_list.size(); i++){
GraphPose* currentPose = &(node_list[i]->graph_pose);
g2o::SE2 optimized_pose = ((g2o::VertexSE2*) sparseOptimizer.vertex(node_list[i]->id))->estimate();
//
if(rot_distance(currentPose->theta, optimized_pose[2]) != 0. || distance(currentPose->x, optimized_pose[0], currentPose->y, optimized_pose[1]) > 0.) {
ROS_INFO("Node %d, pose before optimize: x %f, y %f, t %f", node_list[i]->id, currentPose->x, currentPose->y, currentPose->theta);
ROS_INFO("Node %d, pose after optimize: x %f, y %f, t %f", node_list[i]->id, optimized_pose[0], optimized_pose[1], optimized_pose[2]);
}
currentPose->x = optimized_pose[0];
currentPose->y = optimized_pose[1];
currentPose->theta = optimized_pose[2];
}
sparseOptimizer.clear();
};
double CKinFuTracker::directRotation(const CKeyFrame::tp_ptr pRefeFrame_, const CKeyFrame::tp_ptr pLiveFrame_, SO3Group<double>* pR_rl_)
{
Intr sCamIntr_ = pRefeFrame_->_pRGBCamera->getIntrinsics(2);
Matrix3d K = Matrix3d::Identity();
//note that camera parameters are
K(0, 0) = sCamIntr_.fx;
K(1, 1) = sCamIntr_.fy;
K(0, 2) = sCamIntr_.cx;
K(1, 2) = sCamIntr_.cy;
SO3Group<double> CurR_rl_ = *pR_rl_;
SO3Group<double> PrevR_rl_ = *pR_rl_;
SO3Group<double> MinR_rl_ = *pR_rl_;
Matrix3d R_rl_Kinv = PrevR_rl_.matrix() *K.inverse();
Matrix3d H_rl = K * R_rl_Kinv;
//get R,T of previous
Matrix3d H_rl_t = H_rl.transpose();
Matrix3d R_rl_Kinv_t = R_rl_Kinv.transpose();
const Matd33& devH_rl = pcl::device::device_cast<pcl::device::Matd33> (H_rl_t);
const Matd33& devR_rl_Kinv = pcl::device::device_cast<pcl::device::Matd33> (R_rl_Kinv_t);
double dMinEnergy = numeric_limits<double>::max();
double dPrevEnergy = numeric_limits<double>::max();
dPrevEnergy = energy_direct_radiance_rotation(sCamIntr_, devR_rl_Kinv, devH_rl, _n_rad_origin_2_ref, _n_rad_live[2], _err_live[2]);
dMinEnergy = dPrevEnergy;
//cout << setprecision(15) << dMinEnergy << endl;
for (short sIter = 0; sIter < 5; ++sIter) {
//get R and T
GpuMat gSumBuf = btl::device::direct_rotation(sCamIntr_, devR_rl_Kinv, devH_rl, _n_rad_origin_2_ref, _n_rad_live[2], _err_live[2]);
Mat Buf; gSumBuf.download(Buf);
SO3Group<double> R_rl = btl::utility::extractRFromBuffer<double>((double*)Buf.data);
//cout << Tran_nc.matrix() << endl;
CurR_rl_ = R_rl *PrevR_rl_;
R_rl_Kinv = CurR_rl_.matrix()*K.inverse();
H_rl = K * R_rl_Kinv;
H_rl_t = H_rl.transpose();
R_rl_Kinv_t = R_rl_Kinv.transpose();
double dCurEnergy = energy_direct_radiance_rotation(sCamIntr_, devR_rl_Kinv, devH_rl, _n_rad_origin_2_ref, _n_rad_live[2], _err_live[2]);
//cout << sIter << ": " << dPrevEnergy << " " << dCurEnergy << endl;
if (dCurEnergy < dMinEnergy){
dMinEnergy = dCurEnergy;
MinR_rl_ = CurR_rl_;
}
if (dMinEnergy / dCurEnergy < 0.25){ //divereges
//cout << "Diverge Warning:" << endl;
dCurEnergy = dMinEnergy;
CurR_rl_ = MinR_rl_;
break;
}
PrevR_rl_ = CurR_rl_;
if (fabs(dPrevEnergy / dCurEnergy - 1) < 0.01f){ //converges
//cout << "Converges" << endl;
dCurEnergy = dMinEnergy;
CurR_rl_ = MinR_rl_;
break;
}
dPrevEnergy = dCurEnergy;
}
*pR_rl_ = CurR_rl_;
return dMinEnergy;
}
void PushObject::updateModel(const Eigen::MatrixXd& traj, GRBQuadExpr& objective) {
VectorXd& times = m_problem->m_times;
MatrixXd perts_tk = getSinBasis(times/times.maxCoeff(), fmin(6, times.size()/2));
MatrixXd pinvperts_tk = perts_tk * (perts_tk.transpose() * perts_tk).inverse();
m_exactObjective = simulateTraj2(traj, true); // current value
LOG_INFO_FMT("current val: %.3f", m_exactObjective);
MatrixXd dy_jk(traj.cols(), perts_tk.cols());
double eps = 3e-4; // scale for joint angle change
Matrix3d A;
A << sq(eps/2), eps/2, 1,
0, 0, 1,
sq(eps/2), -eps/2, 1;
Matrix3d Ainv = A.inverse();
MatrixXd grad_tj(traj.rows(), traj.cols());
m_obj = GRBQuadExpr(0);
for (int j = 0; j < traj.cols(); ++j) {
VarVector v;
VectorXd vactual(traj.rows()-1);
for (int t=1; t < traj.rows(); ++t) {
v.push_back(m_problem->m_trajVars.at(t,j));
vactual(t-1) = traj(t,j);
}
for (int k = 0; k < perts_tk.cols(); ++k) {
MatrixXd newTraj = traj;
newTraj.col(j) = traj.col(j) + (eps/2)*perts_tk.col(k);
double plusVal = simulateTraj2(newTraj, false);
newTraj.col(j) = traj.col(j) - (eps/2)*perts_tk.col(k);
double minusVal = simulateTraj2(newTraj, false);
LOG_DEBUG_FMT("joint %i, basis %i, pert vals: %.4e %.4e ", j, k, plusVal-m_exactObjective,minusVal-m_exactObjective);
dy_jk(j,k) = (plusVal - minusVal)/eps;
Vector3d y;
y << plusVal-m_exactObjective, 0, minusVal - m_exactObjective;
Vector3d abc = Ainv*y;
GRBLinExpr q;
int T = traj.rows()-1;
VectorXd pertVec=perts_tk.block(1,k,T, 1);
q.addTerms(pertVec.data(),v.data(), T);
double qactual = pertVec.dot(vactual);
m_obj += fmax(abc(0),0)*(q-qactual)*(q-qactual) + abc(1)*(q-qactual);
}
// grad_tj.col(j) = pinvperts_tk * dy_jk.row(j).transpose();
}
m_obj += m_exactObjective;
// cout << "dy_jk:" << endl;
// cout << dy_jk << endl;
// cout << "grad_tj:" << endl;
// cout << grad_tj << endl;
// m_obj.addTerms(grad_tj.data()+7, m_problem->m_trajVars.m_data.data()+7, (traj.rows()-1)*traj.cols());
// m_obj += m_exactObjective - (grad_tj.middleRows(1, traj.rows()-1).array() * traj.middleRows(1, traj.rows()-1).array()).sum();
objective += m_obj;
}
void GraphSimulator::simulate(int samples, int trajectories, bool interClosures, bool lookForClosures, const Isometry2d& offset)
{
// grid size
int size = 50;
// some parameters for the generation of the samples
int forwardSteps = 3;
double stepLength = 1.0;
Isometry2d maxStepTransform(utility::v2t(Vector3d(forwardSteps * stepLength, 0, 0)));
// Fake sensor for loop-closure detection
double fov = (forwardSteps - 1) << 1;
cout << "FOV: " << fov << endl;
Vector2d grid(size >> 1, size >> 1);
cout << "Grid: " << grid.x() << ", " << grid.y() << endl;
VectorXd probLimits(POSSIBLE_MOTIONS);
for(int i = 0; i < probLimits.size(); ++i)
{
probLimits[i] = (i + 1) / (double) POSSIBLE_MOTIONS;
}
Matrix3d covariance;
covariance.fill(0.);
covariance(0, 0) = _noise[0] * _noise[0];
covariance(1, 1) = _noise[1] * _noise[1];
covariance(2, 2) = _noise[2] * _noise[2];
Matrix3d information = covariance.inverse();
SimNode start;
start.id = 0;
start.real_pose = offset;
start.noisy_pose = offset;
for(short int k = 0; k < trajectories; ++k)
{
_trajectories.push_back(SimGraph());
SimGraph& traj = _trajectories.back();
Poses& poses = traj._poses;
poses.clear();
poses.push_back(start);
// Nodes
while((int) poses.size() < samples)
{
// go straight for some steps ...
for(int i = 1; i < forwardSteps && (int) poses.size() < samples; ++i)
{
SimNode nextPose = generatePose(poses.back(), utility::v2t(Vector3d(stepLength, 0, 0)));
poses.push_back(nextPose);
}
if((int) poses.size() == samples)
{
break;
}
// ... now some other direction
double uniform_value = Noise::uniform(0., 1.);
int direction = 0;
while(probLimits[direction] < uniform_value && direction + 1 < POSSIBLE_MOTIONS)
{
direction++;
}
Isometry2d nextMotion = generateMotion(direction, stepLength);
SimNode nextPose = generatePose(poses.back(), nextMotion);
Isometry2d nextStepFinalPose = nextPose.real_pose * maxStepTransform;
if(fabs(nextStepFinalPose.translation().x()) >= grid[0] || fabs(nextStepFinalPose.translation().y()) >= grid[1])
{
for(int i = 0; i < POSSIBLE_MOTIONS; ++i)
{
nextMotion = generateMotion(i, stepLength);
nextPose = generatePose(poses.back(), nextMotion);
nextStepFinalPose = nextPose.real_pose * maxStepTransform;
if(fabs(nextStepFinalPose.translation().x()) < grid[0] && fabs(nextStepFinalPose.translation().y()) < grid[1])
{
break;
}
}
}
poses.push_back(nextPose);
}
cout << "Added Nodes" << endl;
// Edges
Edges& edges = traj._edges;
edges.clear();
for(size_t i = 1; i < poses.size(); ++i)
{
SimNode& prev = poses[i-1];
SimNode& curr = poses[i];
SimEdge* edge = new SimEdge;
edge->from_id = prev.id;
edge->to_id = curr.id;
edge->real_transform = prev.real_pose.inverse() * curr.real_pose;
edge->noisy_transform = prev.noisy_pose.inverse() * curr.noisy_pose;
edge->information = information;
edges.insert(edge);
prev._connections.insert(edge);
}
cout << "Added Edges" << endl;
// Loop Closures
if(lookForClosures)
{
// Closures
for(int i = poses.size()-1; i >= 0; i--)
{
SimNode& sp = poses[i];
// for(int j = 0; j < i; j+=20)
for(int j = 0; j < i; j+=5)
{
SimNode& candidate = poses[j];
Isometry2d transform = sp.real_pose.inverse() * candidate.real_pose;
double distance = utility::t2v(transform).squaredNorm();
if(fabs(distance) <= fov)
{
SimEdge* loopClosure = new SimEdge;
loopClosure->from_id = sp.id;
loopClosure->to_id = candidate.id;
loopClosure->real_transform = transform;
loopClosure->noisy_transform = transform;
loopClosure->information = information;
edges.insert(loopClosure);
sp._connections.insert(loopClosure);
}
}
}
}
}
cout << "Added Loop Closures" << endl;
// Inter Graph Closures
if(interClosures)
{
Edges& closures = _closures;
for(uint i = 1; i < _trajectories.size(); ++i)
{
SimGraph& t1 = _trajectories[i-1];
SimGraph& t2 = _trajectories[i];
const Poses& g1_poses = t1.poses();
const Poses& g2_poses = t2.poses();
for(uint i = 0; i < g2_poses.size(); i+=5)
{
const SimNode& sp = g2_poses[i];
for(uint j = 0; j < g1_poses.size(); j+=5)
{
const SimNode& candidate = g1_poses[j];
Isometry2d transform = sp.real_pose.inverse() * candidate.real_pose;
double distance = utility::t2v(transform).squaredNorm();
if(fabs(distance) <= fov)
{
SimEdge* graphClosure = new SimEdge;
graphClosure->from_id = sp.id;
graphClosure->to_id = candidate.id;
graphClosure->real_transform = transform;
graphClosure->noisy_transform = transform;
graphClosure->information = information;
closures.insert(graphClosure);
}
}
}
}
cout << "Added Inter Graph Closures" << endl;
}
}
Matrix3d ConsState::strainTensor(Matrix3d F)
{
Matrix3d G = (F.inverse().transpose())*(F.inverse());
return G;
}
