RMatrix::RMatrix(const RMatrix &A)
:ARMatrix(A.numberOfRows(), A.numberOfColumns())
{
_pelm = (LongRealPointer) new LongReal [attr_numberOfRows*attr_numberOfColumns];
(*this) = A;
}
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]);
}
/*! 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 );
}
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];
}
/**
* 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;
}
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;
}
/**
* 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;
}
/**
* 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};
}
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 ];
}
}
}
}
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]);
}
}
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;
}
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;
}
}
}
}
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);
}
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;
}
/**
* 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;
}
/**
* \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;
}
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;
}
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;
}
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]);
}
}
/**
* \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;
}
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];
}
}
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
/**
* 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;
}
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
}
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]);
}
}
}
// 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
// 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));
}
}
}
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;
}
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;
}