Пример #1
0
RMatrix::RMatrix(const RMatrix &A)
:ARMatrix(A.numberOfRows(), A.numberOfColumns())
 {   
    _pelm = (LongRealPointer) new LongReal [attr_numberOfRows*attr_numberOfColumns];

	(*this) = A;
 }
Пример #2
0
void interpolate(const Mesh & mesh, const std::string & dataName, Mesh & pos,
                 bool verbose){
    RMatrix vData; vData.push_back(mesh.exportData(dataName));
    RMatrix viData;
    interpolate(mesh, vData, pos.positions(), viData, verbose);
    pos.addExportData(dataName, viData[0]);
}
Пример #3
0
 /*! Optional: generation of jacobian matrix, uncomment for default behaviour (brute force) */
 void createJacobian( const RVector & model ) {
     RMatrix *J = dynamic_cast < RMatrix * >( jacobian_ );
     if ( jacobian_->rows() != x_.size() || jacobian_->cols() != nc_ ) J->resize( x_.size(), nc_ );
     
     for ( size_t i = 0 ; i < nc_ ; i++ )
         for ( size_t j = 0 ; j < x_.size() ; j++ )
             (*J)[ j ][ i ] = pow( x_[ j ], (double)i );
 }
Пример #4
0
void interpolate(const Mesh & mesh, const RVector & data,
                 const R3Vector & pos,
                 RVector & iData, bool verbose){
    RMatrix vData; vData.push_back(data);
    RMatrix viData;
    interpolate(mesh, vData, pos, viData, verbose);
    iData = viData[0];
}
Пример #5
0
/**
 * Generic 3d transformation. \c m must be a 3x3 matrix.
 */
RVector RVector::transform(const RMatrix& m) {
    RMatrix input;

    input = RMatrix::create3x1(x, y, z);
    RMatrix res = m * input;
    x = res.get(0, 0);
    y = res.get(1, 0);
    z = res.get(2, 0);

    return *this;
}
Пример #6
0
RMatrix RMatrix::operator *(double s) const {
    RMatrix ret = *this;

    for (int rc = 0; rc < ret.getRows(); ++rc) {
        for (int cc = 0; cc < ret.getCols(); ++cc) {
            ret.set(rc, cc, ret.get(rc, cc) * s);
        }
    }

    return ret;
}
Пример #7
0
/**
 * Stream operator for QDebug
 */
QDebug operator<<(QDebug dbg, const RMatrix& m) {
    dbg.nospace() << "RMatrix(";
    for (int rc = 0; rc < m.getRows(); ++rc) {
        for (int cc = 0; cc < m.getCols(); ++cc) {
            dbg.nospace() << m.get(rc, cc);
            if (rc!=m.getRows()-1 || cc!=m.getCols()-1) {
                dbg.nospace() << ",";
            }
        }
    }
    dbg.nospace() << ")";
    return dbg;
}
Пример #8
0
 /**
  * Mix the two parameter sets \f$ \Pi_i \f$ and \f$ \Pi_j \f$
  * from the bra and the ket wavepackets \f$ \Phi\left[\Pi_i\right] \f$
  * and \f$ \Phi^\prime\left[\Pi_j\right] \f$.
  *
  * \param ket The parameter set \f$ \Pi_j \f$ from the ket part wavepacket.
  * \return The mixed parameters \f$ q_0 \f$ and \f$ Q_0 \f$.
  */
 std::pair< RMatrix<D,1>, RMatrix<D,D> >
 mix(const HaWpParamSet<D>& ket) const
 {
     // Mix the parameters
     CMatrix<D,D> Gbra = P_ * Q_.inverse();
     CMatrix<D,D> Gket = ket.P_ * ket.Q_.inverse();
     RMatrix<D,D> G = (Gket - Gbra.adjoint()).imag();
     RMatrix<D,1> g = (Gket*ket.q_ - Gbra.adjoint()*q_).imag();
     RMatrix<D,1> q0 = G.inverse() * g;
     RMatrix<D,D> Q0 = 0.5 * G;
     // We can not avoid the matrix root
     RMatrix<D,D> Qs = Q0.sqrt().inverse();
     return {q0,Qs};
 }
Пример #9
0
void HarmonicModelling::createJacobian( const RVector & model ) {
    //!! jacobian = transpose( A );
    RMatrix * jacobian = dynamic_cast < RMatrix * > ( jacobian_ );
    
    if ( jacobian->rows() != nt_ || jacobian->cols() != np_ ) {
        jacobian->resize( nt_, np_ );

        for ( size_t i = 0 ; i < np_ ; i++ ){
            for ( size_t j = 0 ; j < nt_ ; j++ ){
                (*jacobian)[ j ][ i ] = A_[ i ][ j ];
            }
        }
    }
}
Пример #10
0
void GSystem::calcDerivative_PositionCOMGlobal_Dq(RMatrix &DpDq_)
{
	int i;
	std::list<GBody *>::iterator iter_pbody;
	std::list<GCoordinate *>::iterator iter_pcoord;
	std::vector<SE3> M(pBodies.size());
	std::vector<GConstraintJointLoop::JOINTLOOP_CONSTRAINT_TYPE> jlc_type(pBodies.size());
	Vec3 DpDqi;

	for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {

		// save previous settings
		M[i] = (*iter_pbody)->fwdJointChain.M1;
		jlc_type[i] = (*iter_pbody)->fwdJointChain.jointLoopConstraintType;

		// prerequisites for (*iter_pbody)->getDerivative_PositionCOMGlobal_Dq(...)
		(*iter_pbody)->fwdJointChain.setM(SE3());
		(*iter_pbody)->fwdJointChain.jointLoopConstraintType = GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION;
		(*iter_pbody)->fwdJointChain.update_J();
	}

	// calculate the derivative
	DpDq_.SetZero(3, getNumCoordinates());
	for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
		DpDqi = getDerivative_PositionCOMGlobal_Dq(*iter_pcoord);
		//DpDq_.Push(0, i, convert_to_RMatrix(DpDqi));
		matSet(&DpDq_[3*i], DpDqi.GetArray(), 3);
	}

	// restore the previous settings
	for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
		(*iter_pbody)->fwdJointChain.setM(M[i]);
		(*iter_pbody)->fwdJointChain.setJointLoopConstraintType(jlc_type[i]);
	}
}
Пример #11
0
bool GSystemIK::solveIK_dq(RMatrix &dq, std::vector<GBody*> pbodies_primary, std::vector<GBody*> pbodies_secondary, std::vector<Vec3> p_primary, std::vector<Vec3> p_secondary, std::vector<se3> V_primary, std::vector<se3> V_secondary, std::vector< std::vector<int> > idxC_primary, std::vector< std::vector<int> > idxC_secondary, gReal alpha_primary, gReal alpha_secondary)
{
	RMatrix Jp, Js;		// Jacobian matrices for primary/secondary constraints
	RMatrix Vp, Vs;		// the righthand side of the constraints

	if ( !buildConstrIK_dq(Jp, Vp, pbodies_primary, p_primary, V_primary, idxC_primary) ) return false;
	if ( !buildConstrIK_dq(Js, Vs, pbodies_secondary, p_secondary, V_secondary, idxC_secondary) ) return false;

	dq.SetZero(getNumCoordinates(), 1);
	if ( Jp.RowSize() > 0 ) {
		RMatrix dq0, N;
		dq0 = srInv(Jp, N, alpha_primary) * Vp;
		if ( Js.RowSize() > 0 ) {
			dq = dq0 + N * ( srInv(Js * N, alpha_secondary) * (Vs - Js * dq0) );
		} else {
			dq = dq0;
		}
	} else {
		if ( Js.RowSize() > 0 ) {
			dq = srInv(Js, alpha_secondary) * Vs;
		}
	}

	return true;
}
Пример #12
0
 void operator()(std::size_t begin_pool, std::size_t end_pool)
 {
   int poolStart, poolEnd, maxIndex;
   int ncols = inc.ncol();
   double sumInc;
   
   for (int i = begin_pool; i < end_pool; i++)
   {
     poolStart = poolSize * i;
     poolEnd = poolStart + (poolSize - 1);
     
     for (int col = 0; col < ncols; col++)
     {
       sumInc = 0;
       
       // Collect sum of changes, only one has changed
       for (int j = poolStart; j <= poolEnd; j++)
       {
         sumInc += inc(j, col);
       }
       
       for (int j = poolStart; j <= poolEnd; j++)
       {
         // All weights within a pool are kept the same
         inc(j, col) = sumInc;
       }
     }
   }
 }
Пример #13
0
RVector3 MeshEntity::grad(const RVector3 & xyz, const RVector & u) const {

    RVector3 rst(shape_->rst(xyz));

    RMatrix MdNdL;
    MdNdL.push_back(dNdL(rst, 0));
    MdNdL.push_back(dNdL(rst, 1));
    MdNdL.push_back(dNdL(rst, 2));

    RVector up(u(this->ids()));
    RVector3 gr;
    gr[0] = sum(up * MdNdL.transMult(shape_->invJacobian().col(0)));
    gr[1] = sum(up * MdNdL.transMult(shape_->invJacobian().col(1)));
    gr[2] = sum(up * MdNdL.transMult(shape_->invJacobian().col(2)));
    return gr;
}
LongRealSvd::LongRealSvd(const RMatrix& A)
: attr_U(A.numberOfRows(), MIN(A.numberOfRows(), A.numberOfColumns())), 
  attr_V(A.numberOfColumns(), MIN(A.numberOfRows(), A.numberOfColumns())), 
  attr_sigma(MIN(A.numberOfColumns(), A.numberOfRows()))
 {
    (*this)
    ;   
    svd(A); 
 }
Пример #15
0
bool GSystem::calcProductOfInvMassAndMatrix2(RMatrix &invM_A, const RMatrix &A)
{
	if ( (size_t)A.RowSize() != pCoordinates.size() ) return false;
	invM_A.SetZero(A.RowSize(), A.ColSize());

	int i;
	std::list<GJoint *>::iterator iter_pjoint;
	std::vector<bool> isprescribed(pJoints.size());

	// save current info
	for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
		isprescribed[i] = (*iter_pjoint)->isPrescribed();
	}

	setAllJointsPrescribed(false);

	initDynamicsWithZeroGravityAndVelocity();
	for (i=0; i<A.ColSize(); i++) {
		set_tau(&(A[i*A.RowSize()]));
		calcDynamicsWithZeroGravityAndVelocity();
		get_ddq(&(invM_A[i*invM_A.RowSize()]));
	}

	// restore
	for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
		(*iter_pjoint)->setPrescribed(isprescribed[i]);
	}

	return true;
}
Пример #16
0
/**
 * Changes this vector into its isometric projection.
 * \todo refactor
 */
RVector RVector::isoProject(RS::IsoProjectionType type, bool trueScale) {
    static RMatrix iso =
            RMatrix::create3x3(
                sqrt(3.0), 0.0,        -sqrt(3.0),
                1.0,       2.0,        1.0,
                sqrt(2.0), -sqrt(2.0), sqrt(2.0)
            ) * (1.0 / sqrt(6.0));

    RMatrix input;
    switch (type) {
    case RS::IsoRight:
        input = RMatrix::create3x1(x, y, -z);
        break;
    case RS::IsoRightBack:
        input = RMatrix::create3x1(-x, y, z);
        break;
    case RS::IsoTop:
        input = RMatrix::create3x1(y, z, -x);
        break;
    case RS::IsoBottom:
        input = RMatrix::create3x1(y, z, x);
        break;
    case RS::IsoLeft:
        input = RMatrix::create3x1(z, y, -x);
        break;
    case RS::IsoLeftBack:
        input = RMatrix::create3x1(z, y, x);
        break;
    }

    RMatrix res = iso * input;

    x = res.get(0, 0);
    y = res.get(1, 0);
    z = 0.0;

    if (trueScale) {
        double f = 1.0 / cos(RMath::deg2rad(35.0 + 16.0/60.0));
        x *= f;
        y *= f;
    }

    return *this;
}
Пример #17
0
/**
 * \return The inverse matrix of this matrix \f$A^{-1}\f$ or
 * an empty matrix if this matrix is not invertible.
 */
RMatrix RMatrix::getInverse() const {
    if (cols != rows) {
        return RMatrix();
    }

    RMatrix a = getAppended(createIdentity(cols));
    if (!a.rref()) {
        return RMatrix();
    }

    RMatrix ret(rows, cols);
    for (int r = 0; r < rows; r++) {
        for (int c = 0; c < cols; c++) {
            ret.set(r, c, a.get(r, c + cols));
        }
    }

    return ret;
}
Пример #18
0
bool GSystem::calcProductOfInvMassAndMatrix(RMatrix &invM_A, const RMatrix &A)
{
	if ( (size_t)A.RowSize() != pCoordinates.size() ) return false;
	invM_A.SetZero(A.RowSize(), A.ColSize());

	int i;
	std::list<GJoint *>::iterator iter_pjoint;
	std::vector<bool> isprescribed(pJoints.size());

	// save current info
	for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
		isprescribed[i] = (*iter_pjoint)->isPrescribed();
	}
	Vec3 g = getGravity();

	// set all joint unprescribed and set zero gravity
	setAllJointsPrescribed(false);
	setGravity(Vec3(0,0,0));

	update_joint_local_info_short();

	for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
		(*iter_pbody)->update_base_joint_info();
		(*iter_pbody)->update_T();
		(*iter_pbody)->set_eta_zero();
	}

	for (std::list<GBody *>::reverse_iterator riter_pbody = pBodies.rbegin(); riter_pbody != pBodies.rend(); riter_pbody++) {
		(*riter_pbody)->update_aI();
		(*riter_pbody)->update_Psi();
		(*riter_pbody)->update_Pi();
	}

	for (i=0; i<A.ColSize(); i++) {
		set_ddq(Zeros(pCoordinates.size(),1)); // this isn't necessary for real tree structure systems, but works for the cut joints in closed-loop
		set_tau(&(A[i*A.RowSize()]));

		for (std::list<GBody *>::reverse_iterator riter_pbody = pBodies.rbegin(); riter_pbody != pBodies.rend(); riter_pbody++) {
			(*riter_pbody)->update_aB_zeroV_zeroeta();
			(*riter_pbody)->update_beta_zeroeta();
		}

		for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
			(*iter_pbody)->update_ddq();
			(*iter_pbody)->update_dV(true);
		}

		get_ddq(&(invM_A[i*invM_A.RowSize()]));
	}

	// restore
	for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
		(*iter_pjoint)->setPrescribed(isprescribed[i]);
	}
	setGravity(g);

	return true;
}
Пример #19
0
bool LULinearSolver<Matrix, Vector>::addJMInvJt(RMatrix& result, JMatrix& J, double fact)
{
    const unsigned int Jrows = J.rowSize();
    const unsigned int Jcols = J.colSize();
    if (Jcols != (unsigned int)this->currentGroup->systemMatrix->rowSize())
    {
        serr << "LULinearSolver::addJMInvJt ERROR: incompatible J matrix size." << sendl;
        return false;
    }

    if (!Jrows) return false;
    computeMinv();

    const typename JMatrix::LineConstIterator jitend = J.end();
    for (typename JMatrix::LineConstIterator jit1 = J.begin(); jit1 != jitend; ++jit1)
    {
        int row1 = jit1->first;
        for (typename JMatrix::LineConstIterator jit2 = jit1; jit2 != jitend; ++jit2)
        {
            int row2 = jit2->first;
            double acc = 0.0;
            for (typename JMatrix::LElementConstIterator i1 = jit1->second.begin(), i1end = jit1->second.end(); i1 != i1end; ++i1)
            {
                int col1 = i1->first;
                double val1 = i1->second;
                for (typename JMatrix::LElementConstIterator i2 = jit2->second.begin(), i2end = jit2->second.end(); i2 != i2end; ++i2)
                {
                    int col2 = i2->first;
                    double val2 = i2->second;
                    acc += val1 * getMinvElement(col1,col2) * val2;
                }
            }
            acc *= fact;
            //sout << "W("<<row1<<","<<row2<<") += "<<acc<<" * "<<fact<<sendl;
            result.add(row1,row2,acc);
            if (row1!=row2)
                result.add(row2,row1,acc);
        }
    }
    return true;
}
Пример #20
0
void GSystem::calcDerivative_MomentumGlobal_Dq_Ddq(std::vector<GCoordinate*> pCoordinates_, RMatrix &DHgDq_, RMatrix &DHgDdq_)
{
	int i;
	std::list<GBody *>::iterator iter_pbody;
	std::vector<GCoordinate *>::iterator iter_pcoord;
	std::vector<SE3> M(pBodies.size());
	std::vector<GConstraintJointLoop::JOINTLOOP_CONSTRAINT_TYPE> jlc_type(pBodies.size());
	dse3 DHDqi, DHDdqi;

	for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {

		// save previous settings
		M[i] = (*iter_pbody)->fwdJointChain.M1;
		jlc_type[i] = (*iter_pbody)->fwdJointChain.jointLoopConstraintType;

		// prerequisites for (*iter_pbody)->getDerivative_MomentumGlobal_Dq(...) and (*iter_pbody)->getDerivative_MomentumGlobal_Ddq(...)
		(*iter_pbody)->fwdJointChain.setM(SE3());
		(*iter_pbody)->fwdJointChain.jointLoopConstraintType = GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION;
		(*iter_pbody)->fwdJointChain.update_J();
	}

	// calculate the derivatives
	DHgDq_.SetZero(6, int(pCoordinates_.size()));
	DHgDdq_.SetZero(6, int(pCoordinates_.size()));
	for (i=0, iter_pcoord = pCoordinates_.begin(); iter_pcoord != pCoordinates_.end(); i++, iter_pcoord++) {
		DHDqi = getDerivative_MomentumGlobal_Dq(*iter_pcoord);
		DHDdqi = getDerivative_MomentumGlobal_Ddq(*iter_pcoord);
		//DHgDq_.Push(0, i, convert_to_RMatrix(DHDqi));
		//DHgDdq_.Push(0, i, convert_to_RMatrix(DHDdqi));
		matSet(&DHgDq_[6*i], DHDqi.GetArray(), 6);
		matSet(&DHgDdq_[6*i], DHDdqi.GetArray(), 6);
	}

	// restore the previous settings
	for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
		(*iter_pbody)->fwdJointChain.setM(M[i]);
		(*iter_pbody)->fwdJointChain.setJointLoopConstraintType(jlc_type[i]);
	}
}
Пример #21
0
/**
 * \return \f$A \cdot W\f$
 * This matrix is not affected.
 */
RMatrix RMatrix::multiplyWith(const RMatrix& w) const {
    RMatrix r(rows, w.cols);

    for (int cc = 0; cc < r.cols; ++cc) {
        for (int rc = 0; rc < r.rows; ++rc) {
            for (int i = 0; i < cols; ++i) {
                r.set(rc, cc, r.get(rc, cc) + get(rc, i) * w.get(i, cc));
            }
        }
    }

    return r;
}
Пример #22
0
void GSystem::calcDerivative_PositionCOMGlobal_Dq_2(RMatrix &DpDq_)
{
	int i;
	std::list<GCoordinate *>::iterator iter_pcoord;
	Vec3 DpDqi;

	DpDq_.SetZero(3, getNumCoordinates());
	for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
		DpDqi = getDerivative_PositionCOMGlobal_Dq_2(*iter_pcoord);
		DpDq_(0,i) = DpDqi[0];
		DpDq_(1,i) = DpDqi[1];
		DpDq_(2,i) = DpDqi[2];
	}
}
Пример #23
0
void MVS::DecomposeProjectionMatrix(const PMatrix& P, RMatrix& R, CMatrix& C)
{
	#ifndef _RELEASE
	KMatrix K;
	DecomposeProjectionMatrix(P, K, R, C);
	ASSERT(K.IsEqual(Matrix3x3::IDENTITY));
	#endif
	// extract camera center as the right null vector of P
	const Vec4 hC(P.RightNullVector());
	C = (const CMatrix&)hC * INVERT(hC[3]);
	// get rotation
	const cv::Mat mP(3,4,cv::DataType<REAL>::type,(void*)P.val);
	mP(cv::Rect(0,0, 3,3)).copyTo(R);
	ASSERT(R.IsValid());
} // DecomposeProjectionMatrix
Пример #24
0
/**
 * Appends matrix \c v to the right side of this matrix
 * and returns the new matrix. This matrix is not affected.
 *
 * \param v the matrix to append to this matrix
 *
 */
RMatrix RMatrix::getAppended(const RMatrix& v) const {
    if (rows != v.rows) {
        return RMatrix();
    }

    RMatrix r(rows, cols + v.cols);

    for (int rc = 0; rc < rows; ++rc) {
        for (int cc = 0; cc < cols; ++cc) {
            r.set(rc, cc, get(rc, cc));
        }
        for (int cc = cols; cc < cols + v.cols; ++cc) {
            r.set(rc, cc, v.get(rc, cc - cols));
        }
    }

    return r;
}
Пример #25
0
void GSystem::getEquationsOfMotion(RMatrix &M, RMatrix &b)
{
	RMatrix ddq = get_ddq(), tau = get_tau(); // save current ddq and tau
	int n = getNumCoordinates();
	M.ReNew(n,n);
	set_ddq(Zeros(n,1));
	GSystem::calcInverseDynamics();
	b = get_tau();
	for (int i=0; i<n; i++) {
		RMatrix unit = Zeros(n,1);
		unit[i] = 1;
		set_ddq(unit);
		GSystem::calcInverseDynamics();
		get_tau(&M[i*n]);
		for (int j=0; j<n; j++) {
			M[i*n+j] -= b[j];
		}
	}
	set_ddq(ddq); set_tau(tau); // restore ddq and tau
}
Пример #26
0
void interpolate(const Mesh & mesh, Mesh & qmesh, bool verbose){
    RMatrix cellData;
    RMatrix nodeData;
    std::vector< std::string > cellDataNames;
    std::vector< std::string > nodeDataNames;

    for (std::map< std::string, RVector >::const_iterator it = mesh.exportDataMap().begin();
          it != mesh.exportDataMap().end(); it ++){

        if (it->second.size() == mesh.nodeCount()){
            if (verbose) std::cout << " interpolate node data: " << it->first << std::endl;
            nodeData.push_back(it->second);
            nodeDataNames.push_back(it->first);
        } else if (it->second.size() == mesh.cellCount()){
            if (verbose) std::cout << " interpolate cell data: " << it->first << std::endl;
            cellData.push_back(it->second);
            cellDataNames.push_back(it->first);
        } else {
            if (verbose) std::cout << " omit unknown data: " << it->first << " " <<
                it->second.size()<< std::endl;
        }
    }
    
    if (cellData.rows() > 0){
        RMatrix qCellData;
        interpolate(mesh, cellData, qmesh.cellCenter(), qCellData, verbose) ;
        for (uint i= 0; i < cellData.rows(); i ++){
            qmesh.addExportData(cellDataNames[i], qCellData[i]);
        }
    }
    if (nodeData.rows() > 0){
        RMatrix qNodeData;
        interpolate(mesh, nodeData, qmesh.positions(), qNodeData, verbose) ;
        for (uint i= 0; i < nodeData.rows(); i ++){
            qmesh.addExportData(nodeDataNames[i], qNodeData[i]);
        }
    }
}
Пример #27
0
// decomposition of projection matrix into KR[I|-C]: internal calibration ([3,3]), rotation ([3,3]) and translation ([3,1])
// (comparable with OpenCV: normalized cv::decomposeProjectionMatrix)
void MVS::DecomposeProjectionMatrix(const PMatrix& P, KMatrix& K, RMatrix& R, CMatrix& C)
{
	// extract camera center as the right null vector of P
	const Vec4 hC(P.RightNullVector());
	C = (const CMatrix&)hC * INVERT(hC[3]);
	// perform RQ decomposition
	const cv::Mat mP(3,4,cv::DataType<REAL>::type,(void*)P.val);
	cv::RQDecomp3x3(mP(cv::Rect(0,0, 3,3)), K, R);
	// normalize calibration matrix
	K *= INVERT(K(2,2));
	// ensure positive focal length
	if (K(0,0) < 0) {
		ASSERT(K(1,1) < 0);
		NEGATE(K(0,0));
		NEGATE(K(1,1));
		NEGATE(K(0,1));
		NEGATE(K(0,2));
		NEGATE(K(1,2));
		(TMatrix<REAL,2,3>&)R *= REAL(-1);
	}
	ASSERT(R.IsValid());
} // DecomposeProjectionMatrix
Пример #28
0
 // function call operator that work for the specified range (begin/end)
 void operator()(std::size_t begin, std::size_t end) {
    for (std::size_t i = begin; i < end; i++) {
       for (std::size_t j = 0; j < i; j++) {
          
          // rows we will operate on
          RMatrix<double>::Row row1 = mat.row(i);
          RMatrix<double>::Row row2 = mat.row(j);
          
          // compute the average using std::tranform from the STL
          std::vector<double> avg(row1.length());
          std::transform(row1.begin(), row1.end(), // input range 1
                         row2.begin(),             // input range 2
                         avg.begin(),              // output range 
                         average);                 // function to apply
            
          // calculate divergences
          double d1 = kl_divergence(row1.begin(), row1.end(), avg.begin());
          double d2 = kl_divergence(row2.begin(), row2.end(), avg.begin());
             
          // write to output matrix
          rmat(i,j) = sqrt(.5 * (d1 + d2));
       }
    }
 }
Пример #29
0
int main( int argc, char *argv [] )
{
    std::string dataFile( NOT_DEFINED );
    double errPerc = 3.0, lambda = 20.0, lambdaIP = 1.0, maxDepth = 0.0;
    int nlay = 30, maxIter = 20;
    bool verbose = false, lambdaOpt = false, isBlocky = false, isRobust = false, optimizeChi1 = false;

    OptionMap oMap;
    oMap.setDescription("DC1dsmooth - smooth 1d dc resistivity inversion");
    oMap.add( verbose,      "v" , "verbose"       , "Verbose output." );
    oMap.add( lambdaOpt,    "O" , "OptimizeLambda", "Optimize model smoothness using L-curve." );
    oMap.add( isRobust,     "R" , "RobustData"    , "Robust (L1) data weighting." );
    oMap.add( isBlocky,     "B" , "BlockyModel"   , "Blocky (L1) model constraints." );
    oMap.add( optimizeChi1, "C" , "OptimizeChi1"  , "Optimize lambda subject to chi^2=1." );
    oMap.add( maxIter,      "i:", "maxIter"       , "Maximum iteration number." );
    oMap.add( lambda,       "l:", "lambda"        , "Regularization strength lambda." );
    oMap.add( lambdaIP,     "r:", "lambdaIP"      , "Regularization strength lambda for IP." );
    oMap.add( errPerc,      "e:", "error"         , "Error percentage" );
    oMap.add( nlay,         "n:", "nLay"          , "Number of layers" );
    oMap.add( maxDepth,     "d:", "maxDepth"      , "Maximum depth" );
    oMap.addLastArg( dataFile, "Datafile" );
    oMap.parse( argc, argv );

    /*! Data: read data file from column file */
    RMatrix abmnr; 
    loadMatrixCol( abmnr, dataFile ); //! read data
    RVector ab2(  abmnr[ 0 ] );        //! first column
    RVector mn2(  abmnr[ 1 ] );        //! second column
    RVector rhoa( abmnr[ 2 ] );        //! third column

    /*! Define discretization according to AB/2 */
    if ( maxDepth == 0.0 ) {
        maxDepth = max( ab2 ) / 2;   //! rule of thumb
        std::cout << "Maximum depth estimated to " << maxDepth << std::endl;
    }
    RVector thk( nlay - 1, maxDepth / ( nlay - 1 ) );
    thk *= ( maxDepth / sum( thk ) );
    RVector model( nlay, median( rhoa ) );

    /*! Transformations: log for app. resisivity and thickness, logLU for resistivity */
//    TransLogLU< RVector > transRho( lbound, ubound );
    TransLog< RVector > transRho;
    TransLog< RVector > transRhoa;

    /*! Forward operator, transformations and constraints */
    DC1dRhoModelling f( thk, ab2, mn2, false );
    f.region( 0 )->setTransModel( transRho );

    /*! Error estimation */
    double current = 0.1, errVolt = 0;//1e-4;
    RVector voltage( rhoa * f( RVector( 3, 1.0 ) ) * current );
    RVector error = errVolt / voltage + errPerc / 100.0;
    vcout << "error min/max = " << min( error ) << "/" << max( error ) << std::endl;

    /*! Set up inversion with full matrix, data and forward operator */
    RInversion inv( rhoa, f, verbose );
    inv.setTransData( transRhoa );
    inv.setRelativeError( error );
    inv.setLambda( lambda );
    inv.setModel( model );
    inv.setBlockyModel( isBlocky );
    inv.setRobustData( isRobust );
    inv.setOptimizeLambda( lambdaOpt );
    inv.setMaxIter( maxIter );
    inv.setDeltaPhiAbortPercent( 0.5 );
    inv.stopAtChi1( false );

    if ( optimizeChi1 ) {
        model = inv.runChi1( 0.1 );
    } else {
        model = inv.run();
    }
    if ( verbose ) std::cout << "model = " << model << std::endl;
    save( model, "resistivity.vec" );
    save( thk, "thickness.vec" );
    save( inv.response(), "response.vec" );
    
    if ( abmnr.cols() > 3 ) {
        if ( verbose ) std::cout << "Found ip values, doing ip inversion" << std::endl;
        //! imaginary apparent resistivity
        RVector rhoai( rhoa * sin( abmnr[ 3 ] / 1000 ) );
        //! linear modelling operator using the amplitude jacobian
        Mesh mesh( createMesh1D( thk.size() ) );
        LinearModelling fIP( mesh, f.jacobian(), verbose );
        fIP.region( 0 )->setTransModel( transRho );
        //! IP (imaginary resistivity) inversion using fIP
        RInversion invIP( rhoai, fIP, verbose );
        invIP.setAbsoluteError( rhoai / abmnr[ 3 ] * 1.0 );
        invIP.setBlockyModel( isBlocky );
        invIP.setRobustData( isRobust );
        invIP.setLambda( lambdaIP );
        invIP.setRecalcJacobian( false );
        invIP.stopAtChi1( false );
        RVector ipModel( nlay, median( rhoai ) );
        invIP.setModel( ipModel );
        ipModel = invIP.run();
        RVector phase = atan( ipModel / model ) * 1000.;
        save( phase, "phase.vec" );
        RVector aphase = atan( invIP.response() / rhoa ) * 1000.;
        save( aphase, "aphase.vec" ); 
    }

    return EXIT_SUCCESS;
}
Пример #30
0
int main( int argc, char *argv [] ){

    bool lambdaOpt = false, isRobust = false, isBlocky = false, doResolution = false;
    bool useAppPar = false, useTan = false, useWater = false, optimizeChi1 = false;
    double lambda = 1, lbound = 0, ubound = 0, errbabs = 20;
    int maxIter = 10, verboseCount = 0, ctype = 0;
    std::string matFile( "A.mat" ), bFile( "b.vec" );

    OptionMap oMap;
    oMap.setDescription("Description. InvLinearMat - Linear Inversion with given matrix and vector\n");
    oMap.addLastArg( bFile, "Data file" );
    oMap.add( verboseCount, "v" , "verbose", "Verbose/debug/dosave mode (multiple use)." );
    oMap.add( lambdaOpt,    "O" , "OptimizeLambda", "Optimize model smoothness using L-curve." );
    oMap.add( optimizeChi1, "C" , "OptimizeChi1", "Optimize lambda subject to chi^2=1." );
    oMap.add( doResolution, "D" , "doResolution", "Do resolution analysis." );
    oMap.add( isRobust,     "R" , "RobustData", "Robust (L1) data weighting." );
    oMap.add( isBlocky,     "B" , "BlockyModel", "Blocky (L1) model constraints." );
    oMap.add( useTan,       "T" , "useTan", "Use (co-)Tan instead of Log for LU." );
    oMap.add( useAppPar,    "A" , "useAppPar", "Use apparent parameter transformation." );
    oMap.add( useWater,     "W" , "useWater", "Use water content transformation." );
    oMap.add( matFile,      "m:", "matFile", "Matrix file [A.mat]" );
    oMap.add( lambda,       "l:", "lambda", "Regularization strength lambda[100]." );
    oMap.add( lbound,       "b:", "lbound", "Lower parameter bound[0]" );
    oMap.add( ubound,       "u:", "ubound", "Upper parameter bound[0-inactive]" );
    oMap.add( errbabs,      "e:", "error", "Absolute error level" );
    oMap.add( maxIter,      "i:", "maxIter", "Maximum Iteration number" );
    oMap.parse( argc, argv );
    bool verbose = ( verboseCount > 0 ), debug = ( verboseCount > 1 ), dosave = ( verboseCount > 2 );

    RMatrix A;
    if ( ! loadMatrixSingleBin( A, matFile ) ) { std::cerr << "Did not find A.mat!" << std::endl; return EXIT_OPEN_FILE; }
    RVector b; load( b, bFile );
    size_t nModel( A.cols() );
    dcout << "size(A) = " << A.rows() << "x" << nModel << "size(b) = " << b.size() << std::endl;

    RVector Asum( A * RVector( nModel, 1.0 ) );
    RVector xapp( b / Asum );
    DEBUG save( xapp, "xapp.vec" );
    dcout << "apparent x: min/max = " << min( xapp ) << "/" << max( xapp ) << std::endl;

    Mesh mesh( createMesh1D( nModel ) );
    DEBUG mesh.save( "mesh1d.bms" );
    mesh.showInfos();
    for ( size_t i = 0; i < mesh.cellCount(); i ++ ) mesh.cell( i ).setAttribute( 2.0 + i );
    mesh.createNeighbourInfos();

    LinearModelling f( mesh, A, verbose );
    f.regionManager().region( 0 )->setConstraintType( ctype );

    Trans < RVector > * transData;
    Trans < RVector > * transModel;
    if ( useAppPar ) {
        if ( useTan ) {
            if ( ubound <= lbound ) ubound = lbound + 1.0;
            transData = new TransNest< RVector >( *new TransCotLU< RVector >( lbound, ubound ),
                                                  *new TransLinear< RVector >( RVector( xapp / b ) ) );
            transModel = new TransCotLU< RVector >( lbound, ubound );
        } else {
            transData = new TransNest< RVector >( *new TransLogLU< RVector >( lbound, ubound ),
                                                  *new TransLinear< RVector >( RVector( xapp / b ) ) );
            transModel = new TransLogLU< RVector >( lbound, ubound );
        }
    } else {
        transData = new Trans< RVector >( );
        transModel = new Trans< RVector >( );
    }
    /*! set up inversion */
    RInversion inv( b, f, *transData, *transModel, verbose, dosave );

    inv.setRecalcJacobian( false ); //! no need since it is linear
    inv.setLambda( lambda );
    inv.setOptimizeLambda( lambdaOpt );
    inv.setRobustData( isRobust );
    inv.setBlockyModel( isBlocky );
    inv.setMaxIter( maxIter );
    inv.setDeltaPhiAbortPercent( 1.0 );
    RVector error( errbabs / b);
    vcout << "error: min/max = " << min( error ) << "/" << max( error ) << std::endl;
    inv.setError( error );

    RVector x( nModel, mean( xapp ) );
    inv.setModel( x );
    inv.setReferenceModel( x );

    if ( optimizeChi1 ) { x = inv.runChi1(); } else { x = inv.run(); }
    vcout << "x = " << x << std::endl;
    save( x, "x.vec" );

    if ( doResolution ) {
        RVector resolution( nModel );
        RVector resMDiag ( nModel );
        RMatrix resM;
        for ( size_t iModel = 0; iModel < nModel; iModel++ ) {
            resolution = inv.modelCellResolution( iModel );
            resM.push_back( resolution );
            resMDiag[ iModel ] = resolution[ iModel ];
        }
        save( resMDiag, "resMDiag.vec" );
        save( resM, "resolution.matrix" );
    }

    delete transData;
    delete transModel;

    return EXIT_SUCCESS;
}