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();
}
Ejemplo n.º 2
0
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]);
}
Ejemplo n.º 4
0
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;
}
Ejemplo n.º 5
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);
	}
}
Ejemplo n.º 6
0
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;
}
Ejemplo n.º 7
0
/**
*@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;

}
Ejemplo n.º 8
0
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();
  
}
Ejemplo n.º 9
0
    /**
     * 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;
    }
Ejemplo n.º 10
0
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
}
Ejemplo n.º 11
0
//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;
}
Ejemplo n.º 12
0
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;
}
Ejemplo n.º 13
0
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 ;

}
Ejemplo n.º 14
0
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;

}
Ejemplo n.º 15
0
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;
}
Ejemplo n.º 16
0
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();
};
Ejemplo n.º 17
0
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;
}
Ejemplo n.º 18
0
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;
}
Ejemplo n.º 19
0
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;
    }
}
Ejemplo n.º 20
0
Matrix3d ConsState::strainTensor(Matrix3d F)
{
	Matrix3d G = (F.inverse().transpose())*(F.inverse());	
	return G;
}