inline void resize(Eigen::MatrixXd & m,
vcl_size_t new_rows,
vcl_size_t new_cols)
{
m.resize(new_rows, new_cols);
}
IGL_INLINE bool igl::copyleft::cgal::signed_distance_isosurface(
const Eigen::MatrixXd & IV,
const Eigen::MatrixXi & IF,
const double level,
const double angle_bound,
const double radius_bound,
const double distance_bound,
const SignedDistanceType sign_type,
Eigen::MatrixXd & V,
Eigen::MatrixXi & F)
{
using namespace std;
using namespace Eigen;
// default triangulation for Surface_mesher
typedef CGAL::Surface_mesh_default_triangulation_3 Tr;
// c2t3
typedef CGAL::Complex_2_in_triangulation_3<Tr> C2t3;
typedef Tr::Geom_traits GT;//Kernel
typedef GT::Sphere_3 Sphere_3;
typedef GT::Point_3 Point_3;
typedef GT::FT FT;
typedef std::function<FT (Point_3)> Function;
typedef CGAL::Implicit_surface_3<GT, Function> Surface_3;
AABB<Eigen::MatrixXd,3> tree;
tree.init(IV,IF);
Eigen::MatrixXd FN,VN,EN;
Eigen::MatrixXi E;
Eigen::VectorXi EMAP;
WindingNumberAABB< Eigen::Vector3d, Eigen::MatrixXd, Eigen::MatrixXi > hier;
switch(sign_type)
{
default:
assert(false && "Unknown SignedDistanceType");
case SIGNED_DISTANCE_TYPE_UNSIGNED:
// do nothing
break;
case SIGNED_DISTANCE_TYPE_DEFAULT:
case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
hier.set_mesh(IV,IF);
hier.grow();
break;
case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
// "Signed Distance Computation Using the Angle Weighted Pseudonormal"
// [Bærentzen & Aanæs 2005]
per_face_normals(IV,IF,FN);
per_vertex_normals(IV,IF,PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE,FN,VN);
per_edge_normals(
IV,IF,PER_EDGE_NORMALS_WEIGHTING_TYPE_UNIFORM,FN,EN,E,EMAP);
break;
}
Tr tr; // 3D-Delaunay triangulation
C2t3 c2t3 (tr); // 2D-complex in 3D-Delaunay triangulation
// defining the surface
const auto & IVmax = IV.colwise().maxCoeff();
const auto & IVmin = IV.colwise().minCoeff();
const double bbd = (IVmax-IVmin).norm();
const double r = bbd/2.;
const auto & IVmid = 0.5*(IVmax + IVmin);
// Supposedly the implict needs to evaluate to <0 at cmid...
// http://doc.cgal.org/latest/Surface_mesher/classCGAL_1_1Implicit__surface__3.html
Point_3 cmid(IVmid(0),IVmid(1),IVmid(2));
Function fun;
switch(sign_type)
{
default:
assert(false && "Unknown SignedDistanceType");
case SIGNED_DISTANCE_TYPE_UNSIGNED:
fun =
[&tree,&IV,&IF,&level](const Point_3 & q) -> FT
{
int i;
RowVector3d c;
const double sd = tree.squared_distance(
IV,IF,RowVector3d(q.x(),q.y(),q.z()),i,c);
return sd-level;
};
case SIGNED_DISTANCE_TYPE_DEFAULT:
case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
fun =
[&tree,&IV,&IF,&hier,&level](const Point_3 & q) -> FT
{
const double sd = signed_distance_winding_number(
tree,IV,IF,hier,Vector3d(q.x(),q.y(),q.z()));
return sd-level;
};
break;
case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
fun = [&tree,&IV,&IF,&FN,&VN,&EN,&EMAP,&level](const Point_3 & q) -> FT
{
const double sd =
igl::signed_distance_pseudonormal(
tree,IV,IF,FN,VN,EN,EMAP,RowVector3d(q.x(),q.y(),q.z()));
return sd- level;
};
break;
}
Sphere_3 bounding_sphere(cmid, (r+level)*(r+level));
Surface_3 surface(fun,bounding_sphere);
CGAL::Surface_mesh_default_criteria_3<Tr>
criteria(angle_bound,radius_bound,distance_bound);
// meshing surface
CGAL::make_surface_mesh(c2t3, surface, criteria, CGAL::Manifold_tag());
// complex to (V,F)
return igl::copyleft::cgal::complex_to_mesh(c2t3,V,F);
}
int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q, Eigen::VectorXd& _z)
{
int n = _q.size();
const double zer_tol = 1e-5;
const double piv_tol = 1e-8;
int maxiter = 1000;
int err = 0;
if (_q.minCoeff() > 0)
{
// LOG(INFO) << "Trivial solution exists.";
_z = Eigen::VectorXd::Zero(n);
return err;
}
_z = Eigen::VectorXd::Zero(2 * n);
int iter = 0;
double theta = 0;
double ratio = 0;
int leaving = 0;
Eigen::VectorXd Be = Eigen::VectorXd::Constant(n, 1);
Eigen::VectorXd x = _q;
std::vector<int> bas;
std::vector<int> nonbas;
int t = 2 * n + 1;
int entering = t;
bas.clear();
nonbas.clear();
for (int i = 0; i < n; ++i)
{
bas.push_back(i);
}
Eigen::MatrixXd B = -Eigen::MatrixXd::Identity(n, n);
for (int i = 0; i < bas.size(); ++i) {
B.col(bas[i]) = _M.col(bas[i]);
}
x = -B.partialPivLu().solve(_q);
Eigen::VectorXd minuxX = -x;
int lvindex;
double tval = minuxX.maxCoeff(&lvindex);
leaving = bas[lvindex];
bas[lvindex] = t;
Eigen::VectorXd U = Eigen::VectorXd::Zero(n);
for (int i = 0; i < n; ++i)
{
if (x[i] < 0)
U[i] = 1;
}
Be = -(B * U);
x += tval * U;
x[lvindex] = tval;
B.col(lvindex) = Be;
for (iter = 0; iter < maxiter; ++iter)
{
if (leaving == t)
{
break;
}
else if (leaving < n)
{
entering = n + leaving;
Be = Eigen::VectorXd::Zero(n);
Be[leaving] = -1;
}
else
{
entering = leaving - n;
Be = _M.col(entering);
}
Eigen::VectorXd d = B.partialPivLu().solve(Be);
std::vector<int> j;
for (int i = 0; i < n; ++i)
{
if (d[i] > piv_tol)
j.push_back(i);
}
if (j.empty())
{
// err = 2;
break;
}
int jSize = static_cast<int>(j.size());
Eigen::VectorXd minRatio(jSize);
for (int i = 0; i < jSize; ++i)
{
minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]];
}
double theta = minRatio.minCoeff();
std::vector<int> tmpJ;
std::vector<double> tmpMinRatio;
for (int i = 0; i < jSize; ++i)
{
if (x[j[i]] / d[j[i]] <= theta)
{
tmpJ.push_back(j[i]);
tmpMinRatio.push_back(minRatio[i]);
}
}
// if (tmpJ.empty())
// {
// LOG(WARNING) << "tmpJ should never be empty!!!";
// LOG(WARNING) << "dumping data:";
// LOG(WARNING) << "theta:" << theta;
// for (int i = 0; i < jSize; ++i)
// {
// LOG(WARNING) << "x(" << j[i] << "): " << x[j[i]] << "d: " << d[j[i]];
// }
// }
j = tmpJ;
jSize = static_cast<int>(j.size());
if (jSize == 0)
{
err = 4;
break;
}
lvindex = -1;
for (int i = 0; i < jSize; ++i)
{
if (bas[j[i]] == t)
lvindex = i;
}
if (lvindex != -1)
{
lvindex = j[lvindex];
}
else
{
theta = tmpMinRatio[0];
lvindex = 0;
for (int i = 0; i < jSize; ++i)
{
if (tmpMinRatio[i]-theta > piv_tol)
{
theta = tmpMinRatio[i];
lvindex = i;
}
}
lvindex = j[lvindex];
}
leaving = bas[lvindex];
ratio = x[lvindex] / d[lvindex];
bool bDiverged = false;
for (int i = 0; i < n; ++i)
{
if (isnan(x[i]) || isinf(x[i]))
{
bDiverged = true;
break;
}
}
if (bDiverged)
{
err = 4;
break;
}
x = x - ratio * d;
x[lvindex] = ratio;
B.col(lvindex) = Be;
bas[lvindex] = entering;
}
if (iter >= maxiter && leaving != t)
err = 1;
if (err == 0)
{
for (size_t i = 0; i < bas.size(); ++i) {
if (bas[i] < _z.size()) {
_z[bas[i]] = x[i];
}
}
Eigen::VectorXd realZ = _z.segment(0, n);
_z = realZ;
if (!validate(_M, _z, _q))
{
// _z = VectorXd::Zero(n);
err = 3;
}
}
else
{
_z = Eigen::VectorXd::Zero(n); //solve failed, return a 0 vector
}
// if (err == 1)
// LOG(ERROR) << "LCP Solver: Iterations exceeded limit";
// else if (err == 2)
// LOG(ERROR) << "LCP Solver: Unbounded ray";
// else if (err == 3)
// LOG(ERROR) << "LCP Solver: Solver converged with numerical issues. Validation failed.";
// else if (err == 4)
// LOG(ERROR) << "LCP Solver: Iteration diverged.";
return err;
}
/** \todo Document this rather complex function */
void MetaParametersLWR::getCentersAndWidths(const VectorXd& min, const VectorXd& max, Eigen::MatrixXd& centers, Eigen::MatrixXd& widths) const
{
int n_dims = getExpectedInputDim();
assert(min.size()==n_dims);
assert(max.size()==n_dims);
vector<VectorXd> centers_per_dim_local(n_dims);
if (!centers_per_dim_.empty())
{
centers_per_dim_local = centers_per_dim_;
}
else
{
// Centers are not know yet, compute them based on min and max
for (int i_dim=0; i_dim<n_dims; i_dim++)
centers_per_dim_local[i_dim] = VectorXd::LinSpaced(n_bfs_per_dim_[i_dim],min[i_dim],max[i_dim]);
}
// Determine the widths from the centers (separately for each dimension)
vector<VectorXd> widths_per_dim_local(n_dims);
for (int i_dim=0; i_dim<n_dims; i_dim++)
{
VectorXd cur_centers = centers_per_dim_local[i_dim]; // Abbreviation for convenience
int n_centers = cur_centers.size();
VectorXd cur_widths(n_centers);
if (n_centers==1)
{
cur_widths[0] = 1.0;
}
else if (n_centers==2)
{
cur_widths.fill(sqrt(overlap_*(cur_centers[1]-cur_centers[0])));
}
else
{
cur_widths[0] = sqrt(overlap_*fabs(cur_centers[1]-cur_centers[0]));
cur_widths[n_centers-1] = sqrt(overlap_*fabs(cur_centers[n_centers-1]-cur_centers[n_centers-2]));
for (int cc=1; cc<n_centers-1; cc++)
{
double width_left = sqrt(overlap_*fabs(cur_centers[cc-1]-cur_centers[cc]));
double width_right = sqrt(overlap_*fabs(cur_centers[cc+1]-cur_centers[cc]));
// Recommended width is the average of the recommended widths for left and right
cur_widths[cc] = 0.5*(width_left+width_right);
}
}
widths_per_dim_local[i_dim] = cur_widths;
}
VectorXd digit_max(n_dims);
int n_centers = 1;
for (int i_dim=0; i_dim<n_dims; i_dim++)
{
n_centers = n_centers*centers_per_dim_local[i_dim].size();
digit_max[i_dim] = centers_per_dim_local[i_dim].size();
}
VectorXi digit = VectorXi::Zero(n_dims);
centers.resize(n_centers,n_dims);
widths.resize(n_centers,n_dims);
int i_center=0;
while (digit[0]<digit_max(0))
{
for (int i_dim=0; i_dim<n_dims; i_dim++)
{
centers(i_center,i_dim) = centers_per_dim_local[i_dim][digit[i_dim]];
widths(i_center,i_dim) = widths_per_dim_local[i_dim][digit[i_dim]];
}
i_center++;
// Increment last digit by one
digit[n_dims-1]++;
for (int i_dim=n_dims-1; i_dim>0; i_dim--)
{
if (digit[i_dim]>=digit_max[i_dim])
{
digit[i_dim] = 0;
digit[i_dim-1]++;
}
}
}
}
void mouse_drag(int mouse_x, int mouse_y)
{
using namespace igl;
using namespace std;
using namespace Eigen;
if(is_rotating)
{
glutSetCursor(GLUT_CURSOR_CYCLE);
Quaterniond q;
switch(rotation_type)
{
case ROTATION_TYPE_IGL_TRACKBALL:
{
// Rotate according to trackball
igl::trackball<double>(
width,
height,
2.0,
down_camera.m_rotation_conj.coeffs().data(),
down_x,
down_y,
mouse_x,
mouse_y,
q.coeffs().data());
break;
}
case ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP:
{
// Rotate according to two axis valuator with fixed up vector
two_axis_valuator_fixed_up(
width, height,
2.0,
down_camera.m_rotation_conj,
down_x, down_y, mouse_x, mouse_y,
q);
break;
}
default:
break;
}
camera.orbit(q.conjugate());
}
if(is_dragging)
{
push_scene();
push_object();
if(new_leaf_on_drag)
{
assert(s.C.size() >= 1);
// one new node
s.C.conservativeResize(s.C.rows()+1,3);
const int nc = s.C.rows();
assert(s.sel.size() >= 1);
s.C.row(nc-1) = s.C.row(s.sel(0));
// one new bone
s.BE.conservativeResize(s.BE.rows()+1,2);
s.BE.row(s.BE.rows()-1) = RowVector2i(s.sel(0),nc-1);
// select just last node
s.sel.resize(1,1);
s.sel(0) = nc-1;
// reset down_C
down_C = s.C;
new_leaf_on_drag = false;
}
if(new_root_on_drag)
{
// two new nodes
s.C.conservativeResize(s.C.rows()+2,3);
const int nc = s.C.rows();
Vector3d obj;
int nhits = unproject_in_mesh(mouse_x,height-mouse_y,ei,obj);
if(nhits == 0)
{
Vector3d pV_mid = project(Vcen);
obj = unproject(Vector3d(mouse_x,height-mouse_y,pV_mid(2)));
}
s.C.row(nc-2) = obj;
s.C.row(nc-1) = obj;
// select last node
s.sel.resize(1,1);
s.sel(0) = nc-1;
// one new bone
s.BE.conservativeResize(s.BE.rows()+1,2);
s.BE.row(s.BE.rows()-1) = RowVector2i(nc-2,nc-1);
// reset down_C
down_C = s.C;
new_root_on_drag = false;
}
double z = 0;
Vector3d obj,win;
int nhits = unproject_in_mesh(mouse_x,height-mouse_y,ei,obj);
project(obj,win);
z = win(2);
for(int si = 0;si<s.sel.size();si++)
{
const int c = s.sel(si);
Vector3d pc = project((RowVector3d) down_C.row(c));
pc(0) += mouse_x-down_x;
pc(1) += (height-mouse_y)-(height-down_y);
if(nhits > 0)
{
pc(2) = z;
}
s.C.row(c) = unproject(pc);
}
pop_object();
pop_scene();
}
glutPostRedisplay();
}
// Initialize colors to a boring green
void init_C()
{
C.col(0).setConstant(0.4);
C.col(1).setConstant(0.8);
C.col(2).setConstant(0.3);
}
bool GaussianSet::setDensityMatrix(const Eigen::MatrixXd &m)
{
m_density.resize(m.rows(), m.cols());
m_density = m;
return true;
}
/*! \fn inline void symmetryBlocking(Eigen::MatrixXd & matrix, int cavitySize, int ntsirr, int nr_irrep)
* \param[out] matrix the matrix to be block-diagonalized
* \param[in] cavitySize the size of the cavity (size of the matrix)
* \param[in] ntsirr the size of the irreducible portion of the cavity (size of the blocks)
* \param[in] nr_irrep the number of irreducible representations (number of blocks)
*/
inline void symmetryBlocking(Eigen::MatrixXd & matrix, size_t cavitySize, int ntsirr,
int nr_irrep)
{
// This function implements the simmetry-blocking of the PCM
// matrix due to point group symmetry as reported in:
// L. Frediani, R. Cammi, C. S. Pomelli, J. Tomasi and K. Ruud, J. Comput.Chem. 25, 375 (2003)
// u is the character table for the group (t in the paper)
Eigen::MatrixXd u = Eigen::MatrixXd::Zero(nr_irrep, nr_irrep);
for (int i = 0; i < nr_irrep; ++i) {
for (int j = 0; j < nr_irrep; ++j) {
u(i, j) = parity(i&j);
}
}
// Naming of indices:
// a, b, c, d run over the total size of the cavity (N)
// i, j, k, l run over the number of irreps (n)
// p, q, r, s run over the irreducible size of the cavity (N/n)
// Instead of forming U (T in the paper) and then perform the dense
// multiplication, we multiply block-by-block using just the u matrix.
// matrix = U * matrix * Ut; U * Ut = Ut * U = id
// First half-transformation, i.e. first_half = matrix * Ut
Eigen::MatrixXd first_half = Eigen::MatrixXd::Zero(cavitySize, cavitySize);
for (int i = 0; i < nr_irrep; ++i) {
int ioff = i * ntsirr;
for (int k = 0; k < nr_irrep; ++k) {
int koff = k * ntsirr;
for (int j = 0; j < nr_irrep; ++j) {
int joff = j * ntsirr;
double ujk = u(j, k) / nr_irrep;
for (int p = 0; p < ntsirr; ++p) {
int a = ioff + p;
for (int q = 0; q < ntsirr; ++q) {
int b = joff + q;
int c = koff + q;
first_half(a, c) += matrix(a, b) * ujk;
}
}
}
}
}
// Second half-transformation, i.e. matrix = U * first_half
matrix.setZero(cavitySize, cavitySize);
for (int i = 0; i < nr_irrep; ++i) {
int ioff = i * ntsirr;
for (int k = 0; k < nr_irrep; ++k) {
int koff = k * ntsirr;
for (int j = 0; j < nr_irrep; ++j) {
int joff = j * ntsirr;
double uij = u(i, j);
for (int p = 0; p < ntsirr; ++p) {
int a = ioff + p;
int b = joff + p;
for (int q = 0; q < ntsirr; ++q) {
int c = koff + q;
matrix(a, c) += uij * first_half(b, c);
}
}
}
}
}
// Traverse the matrix and discard numerical zeros
for (size_t a = 0; a < cavitySize; ++a) {
for (size_t b = 0; b < cavitySize; ++b) {
if (numericalZero(matrix(a, b))) {
matrix(a, b) = 0.0;
}
}
}
}
/** unittest for oapackage
*
* Returns UNITTEST_SUCCESS if all tests are ok.
*
*/
int oaunittest (int verbose, int writetests = 0, int randval = 0) {
double t0 = get_time_ms ();
const char *bstr = "OA unittest";
cprintf (verbose, "%s: start\n", bstr);
srand (randval);
int allgood = UNITTEST_SUCCESS;
Combinations::initialize_number_combinations (20);
/* constructors */
{
cprintf (verbose, "%s: interaction matrices\n", bstr);
array_link al = exampleArray (2);
Eigen::MatrixXd m1 = array2xfeigen (al);
Eigen::MatrixXd m2 = arraylink2eigen (array2xf (al));
Eigen::MatrixXd dm = m1 - m2;
int sum = dm.sum ();
myassert (sum == 0, "unittest error: construction of interaction matrices\n");
}
cprintf(verbose, "%s: reduceConferenceTransformation\n", bstr);
myassert(unittest_reduceConferenceTransformation()==0, "unittest unittest_reduceConferenceTransformation failed");
/* constructors */
{
cprintf (verbose, "%s: array manipulation operations\n", bstr);
test_array_manipulation (verbose);
}
/* double conference matrices */
{
cprintf (verbose, "%s: double conference matrices\n", bstr);
array_link al = exampleArray (36, verbose);
myassert (al.is_conference (2), "check on double conference design type");
myassert (testLMC0checkDC (al, verbose >= 2), "testLMC0checkDC");
}
/* conference matrices */
{
cprintf (verbose, "%s: conference matrices\n", bstr);
int N = 4;
conference_t ctype (N, N, 0);
arraylist_t kk;
array_link al = ctype.create_root ();
kk.push_back (al);
for (int extcol = 2; extcol < N; extcol++) {
kk = extend_conference (kk, ctype, 0);
}
myassert (kk.size () == 1, "unittest error: conference matrices for N=4\n");
}
{
cprintf (verbose, "%s: generators for conference matrix extensions\n", bstr);
test_conference_candidate_generators (verbose);
}
{
cprintf (verbose, "%s: conference matrix Fvalues\n", bstr);
array_link al = exampleArray (22, 0);
if (verbose >= 2)
al.show ();
if (0) {
std::vector< int > f3 = al.FvaluesConference (3);
if (verbose >= 2) {
printf ("F3: ");
display_vector (f3);
printf ("\n");
}
}
const int N = al.n_rows;
jstructconference_t js (N, 4);
std::vector< int > f4 = al.FvaluesConference (4);
std::vector< int > j4 = js.Jvalues ();
if (verbose >= 2) {
printf ("j4: ");
display_vector (j4);
printf ("\n");
printf ("F4: ");
display_vector (f4);
printf ("\n");
}
myassert (j4[0] == 28, "unittest error: conference matricex F values: j4[0]\n");
myassert (f4[0] == 0, "unittest error: conference matricex F values: f4[0] \n");
myassert (f4[1] == 0, "unittest error: conference matricex F values: j4[1]\n");
}
{
cprintf (verbose, "%s: LMC0 check for arrays in C(4, 3)\n", bstr);
array_link al = exampleArray (28, 1);
if (verbose >= 2)
al.showarray ();
lmc_t r = LMC0check (al, verbose);
if (verbose >= 2)
printf ("LMC0check: result %d\n", r);
myassert (r >= LMC_EQUAL, "LMC0 check\n");
al = exampleArray (29, 1);
if (verbose >= 2)
al.showarray ();
r = LMC0check (al, verbose);
if (verbose >= 2)
printf ("LMC0check: result %d (LMC_LESS %d)\n", r, LMC_LESS);
myassert (r == LMC_LESS, "LMC0 check of example array 29\n");
}
{
cprintf (verbose, "%s: LMC0 check\n", bstr);
array_link al = exampleArray (31, 1);
if (verbose >= 2)
al.showarray ();
conference_transformation_t T (al);
for (int i = 0; i < 80; i++) {
T.randomize ();
array_link alx = T.apply (al);
lmc_t r = LMC0check (alx, verbose);
if (verbose >= 2) {
printfd ("%d: transformed array: r %d\n", i, r);
alx.showarray ();
}
if (alx == al)
myassert (r >= LMC_EQUAL, "result should be LMC_MORE\n");
else {
myassert (r == LMC_LESS, "result should be LMC_LESS\n");
}
}
}
{
cprintf (verbose, "%s: random transformation for conference matrices\n", bstr);
array_link al = exampleArray (19, 1);
conference_transformation_t T (al);
// T.randomizerowflips();
T.randomize ();
conference_transformation_t Ti = T.inverse ();
array_link alx = Ti.apply (T.apply (al));
if (0) {
printf ("input array:\n");
al.showarray ();
T.show ();
printf ("transformed array:\n");
T.apply (al).showarray ();
Ti.show ();
alx.showarray ();
}
myassert (alx == al, "transformation of conference matrix\n");
}
/* constructors */
{
cprintf (verbose, "%s: constructors\n", bstr);
array_transformation_t t;
conference_transformation_t ct;
}
/* J-characteristics */
{
cprintf (verbose, "%s: J-characteristics\n", bstr);
array_link al = exampleArray (8, 1);
const int mm[] = {-1, -1, 0, 0, 8, 16, 0, -1};
for (int jj = 2; jj < 7; jj++) {
std::vector< int > jx = al.Jcharacteristics (jj);
int j5max = vectormax (jx, 0);
if (verbose >= 2) {
printf ("oaunittest: jj %d: j5max %d\n", jj, j5max);
}
if (j5max != mm[jj]) {
printfd ("j5max %d (should be %d)\n", j5max, mm[jj]);
allgood = UNITTEST_FAIL;
return allgood;
}
}
}
{
cprintf (verbose, "%s: array transformations\n", bstr);
const int N = 9;
const int t = 3;
arraydata_t adataX (3, N, t, 4);
array_link al (adataX.N, adataX.ncols, -1);
al.create_root (adataX);
if (checkTransformationInverse (al))
allgood = UNITTEST_FAIL;
if (checkTransformationComposition (al, verbose >= 2))
allgood = UNITTEST_FAIL;
al = exampleArray (5, 1);
if (checkTransformationInverse (al))
allgood = UNITTEST_FAIL;
if (checkTransformationComposition (al))
allgood = UNITTEST_FAIL;
for (int i = 0; i < 15; i++) {
al = exampleArray (18, 0);
if (checkConferenceComposition (al))
allgood = UNITTEST_FAIL;
if (checkConferenceInverse (al))
allgood = UNITTEST_FAIL;
al = exampleArray (19, 0);
if (checkConferenceComposition (al))
allgood = UNITTEST_FAIL;
if (checkConferenceInverse (al))
allgood = UNITTEST_FAIL;
}
}
{
cprintf (verbose, "%s: rank \n", bstr);
const int idx[10] = {0, 1, 2, 3, 4, 6, 7, 8, 9};
const int rr[10] = {4, 11, 13, 18, 16, 4, 4, 29, 29};
for (int ii = 0; ii < 9; ii++) {
array_link al = exampleArray (idx[ii], 0);
myassert (al.is2level (), "unittest error: input array is not 2-level\n");
int r = arrayrankColPivQR (array2xf (al));
int r3 = (array2xf (al)).rank ();
myassert (r == r3, "unittest error: rank of array");
if (verbose >= 2) {
al.showarray ();
printf ("unittest: rank of array %d: %d\n", idx[ii], r);
}
myassert (rr[ii] == r, "unittest error: rank of example matrix\n");
}
}
{
cprintf (verbose, "%s: Doptimize \n", bstr);
const int N = 40;
const int t = 0;
arraydata_t arrayclass (2, N, t, 6);
std::vector< double > alpha (3);
alpha[0] = 1;
alpha[1] = 1;
alpha[2] = 0;
int niter = 5000;
double t00 = get_time_ms ();
DoptimReturn rr = Doptimize (arrayclass, 10, alpha, 0, DOPTIM_AUTOMATIC, niter);
array_t ss[7] = {3, 3, 2, 2, 2, 2, 2};
arraydata_t arrayclassmixed (ss, 36, t, 5);
rr = Doptimize (arrayclassmixed, 10, alpha, 0, DOPTIM_AUTOMATIC, niter);
cprintf (verbose, "%s: Doptimize time %.3f [s] \n", bstr, get_time_ms () - t00);
}
{
cprintf (verbose, "%s: J-characteristics for conference matrix\n", bstr);
array_link al = exampleArray (19, 0);
std::vector< int > j2 = Jcharacteristics_conference (al, 2);
std::vector< int > j3 = Jcharacteristics_conference (al, 3);
myassert (j2[0] == 0, "j2 value incorrect");
myassert (j2[1] == 0, "j2 value incorrect");
myassert (std::abs (j3[0]) == 1, "j3 value incorrect");
if (verbose >= 2) {
al.showarray ();
printf ("j2: ");
display_vector (j2);
printf ("\n");
printf ("j3: ");
display_vector (j3);
printf ("\n");
}
}
{
// test PEC sequence
cprintf (verbose, "%s: PEC sequence\n", bstr);
for (int ii = 0; ii < 5; ii++) {
array_link al = exampleArray (ii, 0);
std::vector< double > pec = PECsequence (al);
printf ("oaunittest: PEC for array %d: ", ii);
display_vector (pec);
printf (" \n");
}
}
{
cprintf (verbose, "%s: D-efficiency test\n", bstr);
// D-efficiency near-zero test
{
array_link al = exampleArray (14);
double D = al.Defficiency ();
std::vector< double > dd = al.Defficiencies ();
printf ("D %f, D (method 2) %f\n", D, dd[0]);
assert (fabs (D - dd[0]) < 1e-4);
}
{
array_link al = exampleArray (15);
double D = al.Defficiency ();
std::vector< double > dd = al.Defficiencies ();
printf ("D %f, D (method 2) %f\n", D, dd[0]);
assert (fabs (D - dd[0]) < 1e-4);
assert (fabs (D - 0.335063) < 1e-3);
}
}
arraydata_t adata (2, 20, 2, 6);
OAextend oaextendx;
oaextendx.setAlgorithm ((algorithm_t)MODE_ORIGINAL, &adata);
std::vector< arraylist_t > aa (adata.ncols + 1);
printf ("OA unittest: create root array\n");
create_root (&adata, aa[adata.strength]);
/** Test extend of arrays **/
{
cprintf (verbose, "%s: extend arrays\n", bstr);
setloglevel (SYSTEM);
for (int kk = adata.strength; kk < adata.ncols; kk++) {
aa[kk + 1] = extend_arraylist (aa[kk], adata, oaextendx);
printf (" extend: column %d->%d: %ld->%ld arrays\n", kk, kk + 1, aa[kk].size (),
aa[kk + 1].size ());
}
if (aa[adata.ncols].size () != 75) {
printf ("extended ?? to %d arrays\n", (int)aa[adata.ncols].size ());
}
myassert (aa[adata.ncols].size () == 75, "number of arrays is incorrect");
aa[adata.ncols].size ();
setloglevel (QUIET);
}
{
cprintf (verbose, "%s: test LMC check\n", bstr);
array_link al = exampleArray (1, 1);
lmc_t r = LMCcheckOriginal (al);
myassert (r != LMC_LESS, "LMC check of array in normal form");
for (int i = 0; i < 20; i++) {
array_link alx = al.randomperm ();
if (alx == al)
continue;
lmc_t r = LMCcheckOriginal (alx);
myassert (r == LMC_LESS, "randomized array cannot be in minimal form");
}
}
{
/** Test dof **/
cprintf (verbose, "%s: test delete-one-factor reduction\n", bstr);
array_link al = exampleArray (4);
cprintf (verbose >= 2, "LMC: \n");
al.reduceLMC ();
cprintf (verbose >= 2, "DOP: \n");
al.reduceDOP ();
}
arraylist_t lst;
{
/** Test different methods **/
cprintf (verbose, "%s: test 2 different methods\n", bstr);
const int s = 2;
arraydata_t adata (s, 32, 3, 10);
arraydata_t adata2 (s, 32, 3, 10);
OAextend oaextendx;
oaextendx.setAlgorithm ((algorithm_t)MODE_ORIGINAL, &adata);
OAextend oaextendx2;
oaextendx2.setAlgorithm ((algorithm_t)MODE_LMC_2LEVEL, &adata2);
printf ("OA unittest: test 2-level algorithm on %s\n", adata.showstr ().c_str ());
std::vector< arraylist_t > aa (adata.ncols + 1);
create_root (&adata, aa[adata.strength]);
std::vector< arraylist_t > aa2 (adata.ncols + 1);
create_root (&adata, aa2[adata.strength]);
setloglevel (SYSTEM);
for (int kk = adata.strength; kk < adata.ncols; kk++) {
aa[kk + 1] = extend_arraylist (aa[kk], adata, oaextendx);
aa2[kk + 1] = extend_arraylist (aa2[kk], adata2, oaextendx2);
printf (" extend: column %d->%d: %ld->%ld arrays, 2-level method %ld->%ld arrays\n", kk,
kk + 1, (long) aa[kk].size (), (long)aa[kk + 1].size (), aa2[kk].size (), aa2[kk + 1].size ());
if (aa[kk + 1] != aa2[kk + 1]) {
printf ("oaunittest: error: 2-level algorithm unequal to original algorithm\n");
exit (1);
}
}
setloglevel (QUIET);
lst = aa[8];
}
{
cprintf (verbose, "%s: rank calculation using rankStructure\n", bstr);
for (int i = 0; i < 27; i++) {
array_link al = exampleArray (i, 0);
if (al.n_columns < 5)
continue;
al = exampleArray (i, 1);
rankStructure rs;
rs.verbose = 0;
int r = array2xf (al).rank ();
int rc = rs.rankxf (al);
if (verbose >= 2) {
printf ("rank of example array %d: %d %d\n", i, r, rc);
if (verbose >= 3) {
al.showproperties ();
}
}
myassert (r == rc, "rank calculations");
}
}
{
cprintf (verbose, "%s: test dtable creation\n", bstr);
for (int i = 0; i < 4; i++) {
array_link al = exampleArray (5);
array_link dtable = createJdtable (al);
}
}
{
cprintf (verbose, "%s: test Pareto calculation\n", bstr);
double t0x = get_time_ms ();
int nn = lst.size ();
for (int k = 0; k < 5; k++) {
for (int i = 0; i < nn; i++) {
lst.push_back (lst[i]);
}
}
Pareto< mvalue_t< long >, long > r = parsePareto (lst, 1);
cprintf (verbose, "%s: test Pareto %d/%d: %.3f [s]\n", bstr, r.number (), r.numberindices (),
(get_time_ms () - t0x));
}
{
cprintf (verbose, "%s: check reduction transformation\n", bstr);
array_link al = exampleArray (6).reduceLMC ();
arraydata_t adata = arraylink2arraydata (al);
LMCreduction_t reduction (&adata);
reduction.mode = OA_REDUCE;
reduction.init_state = COPY;
OAextend oaextend;
oaextend.setAlgorithm (MODE_ORIGINAL, &adata);
array_link alr = al.randomperm ();
array_link al2 = reduction.transformation->apply (al);
lmc_t tmp = LMCcheck (alr, adata, oaextend, reduction);
array_link alx = reduction.transformation->apply (alr);
bool c = alx == al;
if (!c) {
printf ("oaunittest: error: reduction of randomized array failed!\n");
printf ("-- al \n");
al.showarraycompact ();
printf ("-- alr \n");
alr.showarraycompact ();
printf ("-- alx \n");
alx.showarraycompact ();
allgood = UNITTEST_FAIL;
}
}
{
cprintf (verbose, "%s: reduce randomized array\n", bstr);
array_link al = exampleArray (3);
arraydata_t adata = arraylink2arraydata (al);
LMCreduction_t reduction (&adata);
for (int ii = 0; ii < 50; ii++) {
reduction.transformation->randomize ();
array_link al2 = reduction.transformation->apply (al);
array_link alr = al2.reduceLMC ();
if (0) {
printf ("\n reduction complete:\n");
al2.showarray ();
printf (" --->\n");
alr.showarray ();
}
bool c = (al == alr);
if (!c) {
printf ("oaunittest: error: reduction of randomized array failed!\n");
allgood = UNITTEST_FAIL;
}
}
}
/* Calculate symmetry group */
{
cprintf (verbose, "%s: calculate symmetry group\n", bstr);
array_link al = exampleArray (2);
symmetry_group sg = al.row_symmetry_group ();
assert (sg.permsize () == sg.permsize_large ().toLong ());
// symmetry_group
std::vector< int > vv;
vv.push_back (0);
vv.push_back (0);
vv.push_back (1);
symmetry_group sg2 (vv);
assert (sg2.permsize () == 2);
if (verbose >= 2)
printf ("sg2: %ld\n", sg2.permsize ());
assert (sg2.ngroups == 2);
}
/* Test efficiencies */
{
cprintf (verbose, "%s: efficiencies\n", bstr);
std::vector< double > d;
int vb = 1;
array_link al;
if (1) {
al = exampleArray (9, vb);
al.showproperties ();
d = al.Defficiencies (0, 1);
if (verbose >= 2)
printf (" efficiencies: D %f Ds %f D1 %f Ds0 %f\n", d[0], d[1], d[2], d[3]);
if (fabs (d[0] - al.Defficiency ()) > 1e-10) {
printf ("oaunittest: error: Defficiency not good!\n");
allgood = UNITTEST_FAIL;
}
}
al = exampleArray (8, vb);
al.showproperties ();
d = al.Defficiencies ();
if (verbose >= 2)
printf (" efficiencies: D %f Ds %f D1 %f\n", d[0], d[1], d[2]);
if (fabs (d[0] - al.Defficiency ()) > 1e-10) {
printf ("oaunittest: error: Defficiency of examlple array 8 not good!\n");
}
al = exampleArray (13, vb);
if (verbose >= 3) {
al.showarray ();
al.showproperties ();
}
d = al.Defficiencies (0, 1);
if (verbose >= 2)
printf (" efficiencies: D %f Ds %f D1 %f\n", d[0], d[1], d[2]);
if ((fabs (d[0] - 0.939014) > 1e-4) || (fabs (d[3] - 0.896812) > 1e-4) || (fabs (d[2] - 1) > 1e-4)) {
printf ("ERROR: D-efficiencies of example array 13 incorrect! \n");
d = al.Defficiencies (2, 1);
printf (" efficiencies: D %f Ds %f D1 %f Ds0 %f\n", d[0], d[1], d[2], d[3]);
allgood = UNITTEST_FAIL;
exit (1);
}
for (int ii = 11; ii < 11; ii++) {
printf ("ii %d: ", ii);
al = exampleArray (ii, vb);
al.showarray ();
al.showproperties ();
d = al.Defficiencies ();
if (verbose >= 2)
printf (" efficiencies: D %f Ds %f D1 %f\n", d[0], d[1], d[2]);
}
}
{
cprintf (verbose, "%s: test robustness\n", bstr);
array_link A (0, 8, 0);
printf ("should return an error\n ");
A.Defficiencies ();
A = array_link (1, 8, 0);
printf ("should return an error\n ");
A.at (0, 0) = -2;
A.Defficiencies ();
}
{
cprintf (verbose, "%s: test nauty\n", bstr);
array_link alr = exampleArray (7, 0);
if (unittest_nautynormalform (alr, 1) == 0) {
printf ("oaunittest: error: unittest_nautynormalform returns an error!\n");
}
}
#ifdef HAVE_BOOST
if (writetests) {
cprintf (verbose, "OA unittest: reading and writing of files\n");
boost::filesystem::path tmpdir = boost::filesystem::temp_directory_path ();
boost::filesystem::path temp = boost::filesystem::unique_path ("test-%%%%%%%.oa");
const std::string tempstr = (tmpdir / temp).native ();
if (verbose >= 2)
printf ("generate text OA file: %s\n", tempstr.c_str ());
int nrows = 16;
int ncols = 8;
int narrays = 10;
arrayfile_t afile (tempstr.c_str (), nrows, ncols, narrays, ATEXT);
for (int i = 0; i < narrays; i++) {
array_link al (nrows, ncols, array_link::INDEX_DEFAULT);
afile.append_array (al);
}
afile.closefile ();
arrayfile_t af (tempstr.c_str (), 0);
std::cout << " " << af.showstr () << std::endl;
af.closefile ();
// check read/write of binary file
arraylist_t ll0;
ll0.push_back (exampleArray (7));
ll0.push_back (exampleArray (7).randomcolperm ());
writearrayfile (tempstr.c_str (), ll0, ABINARY);
arraylist_t ll = readarrayfile (tempstr.c_str ());
myassert (ll0.size () == ll.size (), "read and write of arrays: size of list");
for (size_t i = 0; i < ll0.size (); i++) {
myassert (ll0[i] == ll[i], "read and write of arrays: array unequal");
}
ll0.resize (0);
ll0.push_back (exampleArray (24));
writearrayfile (tempstr.c_str (), ll0, ABINARY_DIFFZERO);
ll = readarrayfile (tempstr.c_str ());
myassert (ll0.size () == ll.size (), "read and write of arrays: size of list");
for (size_t i = 0; i < ll0.size (); i++) {
myassert (ll0[i] == ll[i], "read and write of arrays: array unequal");
}
}
#endif
{
cprintf (verbose, "OA unittest: test nauty\n");
array_link al = exampleArray (5, 2);
arraydata_t arrayclass = arraylink2arraydata (al);
for (int i = 0; i < 20; i++) {
array_link alx = al;
alx.randomperm ();
array_transformation_t t1 = reduceOAnauty (al);
array_link alr1 = t1.apply (al);
array_transformation_t t2 = reduceOAnauty (alx);
array_link alr2 = t2.apply (alx);
if (alr1 != alr2)
printf ("oaunittest: error: Nauty reductions unequal!\n");
allgood = UNITTEST_FAIL;
}
}
cprintf (verbose, "OA unittest: complete %.3f [s]!\n", (get_time_ms () - t0));
cprintf (verbose, "OA unittest: also run ptest.py to perform checks!\n");
if (allgood) {
printf ("OA unittest: all tests ok\n");
return UNITTEST_SUCCESS;
} else {
printf ("OA unittest: ERROR!\n");
return UNITTEST_FAIL;
}
}
void NewVizManager::animatePath(const ItompTrajectoryConstPtr& trajectory,
const robot_state::RobotStatePtr& robot_state, bool is_best)
{
if (!is_best)
return;
// marker array
visualization_msgs::MarkerArray ma;
std::vector<std::string> link_names = robot_model_->getMoveitRobotModel()->getLinkModelNames();
std_msgs::ColorRGBA color = colors_[WHITE];
color.a = 1.0;
ros::Duration dur(3600.0);
for (unsigned int point = 0; point < trajectory->getNumPoints(); ++point)
{
ma.markers.clear();
const Eigen::MatrixXd mat = trajectory->getElementTrajectory(
ItompTrajectory::COMPONENT_TYPE_POSITION,
ItompTrajectory::SUB_COMPONENT_TYPE_JOINT)->getTrajectoryPoint(point);
robot_state->setVariablePositions(mat.data());
std::string ns = "frame_" + boost::lexical_cast<std::string>(point);
robot_state->getRobotMarkers(ma, link_names, color, ns, dur);
for (int i = 0; i < ma.markers.size(); ++i)
ma.markers[i].mesh_use_embedded_materials = true;
vis_marker_array_publisher_path_.publish(ma);
}
// MotionPlanning -> Planned Path -> trajectory
moveit_msgs::DisplayTrajectory display_trajectory;
int num_all_joints = robot_state->getVariableCount();
ElementTrajectoryConstPtr joint_trajectory = trajectory->getElementTrajectory(ItompTrajectory::COMPONENT_TYPE_POSITION,
ItompTrajectory::SUB_COMPONENT_TYPE_JOINT);
robot_trajectory::RobotTrajectoryPtr response_trajectory = boost::make_shared<robot_trajectory::RobotTrajectory>(robot_model_->getMoveitRobotModel(), "");
robot_state::RobotState ks = *robot_state;
std::vector<double> positions(num_all_joints);
double dt = trajectory->getDiscretization();
// TODO:
int num_return_points = joint_trajectory->getNumPoints();
for (std::size_t i = 0; i < num_return_points; ++i)
{
for (std::size_t j = 0; j < num_all_joints; j++)
{
positions[j] = (*joint_trajectory)(i, j);
}
ks.setVariablePositions(&positions[0]);
// TODO: copy vel/acc
ks.update();
response_trajectory->addSuffixWayPoint(ks, dt);
}
moveit_msgs::RobotTrajectory trajectory_msg;
response_trajectory->getRobotTrajectoryMsg(trajectory_msg);
display_trajectory.trajectory.push_back(trajectory_msg);
ros::NodeHandle node_handle;
static ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/itomp_planner/display_planned_path", 1, true);
display_publisher.publish(display_trajectory);
}
IGL_INLINE void igl::signed_distance(
const Eigen::MatrixXd & P,
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
const SignedDistanceType sign_type,
Eigen::VectorXd & S,
Eigen::VectorXi & I,
Eigen::MatrixXd & C,
Eigen::MatrixXd & N)
{
using namespace Eigen;
using namespace std;
assert(V.cols() == 3 && "V should have 3d positions");
assert(P.cols() == 3 && "P should have 3d positions");
// Only unsigned distance is supported for non-triangles
if(sign_type != SIGNED_DISTANCE_TYPE_UNSIGNED)
{
assert(F.cols() == 3 && "F should have triangles");
}
// Prepare distance computation
AABB<MatrixXd,3> tree;
tree.init(V,F);
Eigen::MatrixXd FN,VN,EN;
Eigen::MatrixXi E;
Eigen::VectorXi EMAP;
WindingNumberAABB<Eigen::Vector3d> hier;
switch(sign_type)
{
default:
assert(false && "Unknown SignedDistanceType");
case SIGNED_DISTANCE_TYPE_UNSIGNED:
// do nothing
break;
case SIGNED_DISTANCE_TYPE_DEFAULT:
case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
hier.set_mesh(V,F);
hier.grow();
break;
case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
// "Signed Distance Computation Using the Angle Weighted Pseudonormal"
// [Bærentzen & Aanæs 2005]
per_face_normals(V,F,FN);
per_vertex_normals(V,F,PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE,FN,VN);
per_edge_normals(
V,F,PER_EDGE_NORMALS_WEIGHTING_TYPE_UNIFORM,FN,EN,E,EMAP);
N.resize(P.rows(),3);
break;
}
S.resize(P.rows(),1);
I.resize(P.rows(),1);
C.resize(P.rows(),3);
for(int p = 0;p<P.rows();p++)
{
const RowVector3d q = P.row(p);
double s,sqrd;
RowVector3d c;
int i=-1;
switch(sign_type)
{
default:
assert(false && "Unknown SignedDistanceType");
case SIGNED_DISTANCE_TYPE_UNSIGNED:
s = 1.;
sqrd = tree.squared_distance(V,F,q,i,c);
break;
case SIGNED_DISTANCE_TYPE_DEFAULT:
case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
signed_distance_winding_number(tree,V,F,hier,q,s,sqrd,i,c);
break;
case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
{
Eigen::RowVector3d n;
signed_distance_pseudonormal(tree,V,F,FN,VN,EN,EMAP,q,s,sqrd,i,c,n);
N.row(p) = n;
break;
}
}
I(p) = i;
S(p) = s*sqrt(sqrd);
C.row(p) = c;
}
}
void display()
{
using namespace igl;
using namespace std;
using namespace Eigen;
const float back[4] = {30.0/255.0,30.0/255.0,50.0/255.0,0};
glClearColor(back[0],back[1],back[2],0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
if(is_animating)
{
double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION;
if(t > 1)
{
t = 1;
is_animating = false;
}
Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized();
auto & camera = s.camera;
camera.orbit(q.conjugate());
}
glEnable(GL_DEPTH_TEST);
glEnable(GL_NORMALIZE);
lights();
push_scene();
// Draw a nice floor
glEnable(GL_DEPTH_TEST);
glPushMatrix();
const double floor_offset =
-2./bbd*(V.col(1).maxCoeff()-Vmid(1));
glTranslated(0,floor_offset,0);
const float GREY[4] = {0.5,0.5,0.6,1.0};
const float DARK_GREY[4] = {0.2,0.2,0.3,1.0};
glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
draw_floor(GREY,DARK_GREY);
glPopMatrix();
push_object();
// Set material properties
glDisable(GL_COLOR_MATERIAL);
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, SILVER_AMBIENT);
glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, SILVER_DIFFUSE );
glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, SILVER_SPECULAR);
glMaterialf (GL_FRONT_AND_BACK, GL_SHININESS, 128);
typedef std::vector<
Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> >
RotationList;
RotationList dQ(BE.rows(),Quaterniond::Identity()),vQ;
vector<Vector3d> vT;
Matrix3d A = Matrix3d::Identity();
for(int e = 0;e<BE.rows();e++)
{
dQ[e] = AngleAxisd((sin(get_seconds()+e))*0.06*PI,A.col(e%3));
}
forward_kinematics(C,BE,P,dQ,vQ,vT);
const int dim = C.cols();
MatrixXd T(BE.rows()*(dim+1),dim);
for(int e = 0;e<BE.rows();e++)
{
Affine3d a = Affine3d::Identity();
a.translate(vT[e]);
a.rotate(vQ[e]);
T.block(e*(dim+1),0,dim+1,dim) =
a.matrix().transpose().block(0,0,dim+1,dim);
}
if(wireframe)
{
glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
}
glLineWidth(1.0);
MatrixXd U = M*T;
per_face_normals(U,F,N);
draw_mesh(U,F,N);
glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
if(skeleton_on_top)
{
glDisable(GL_DEPTH_TEST);
}
switch(skel_style)
{
default:
case SKEL_STYLE_TYPE_3D:
draw_skeleton_3d(C,BE,T,MAYA_VIOLET,bbd*0.5);
break;
case SKEL_STYLE_TYPE_VECTOR_GRAPHICS:
draw_skeleton_vector_graphics(C,BE,T);
break;
}
pop_object();
pop_scene();
report_gl_error();
TwDraw();
glutSwapBuffers();
glutPostRedisplay();
}
inline vcl_size_t size2(Eigen::MatrixXd const & m) { return m.cols(); }
inline vcl_size_t size1(Eigen::MatrixXd const & m) { return static_cast<vcl_size_t>(m.rows()); }
Eigen::MatrixXd ComputeP_NormalizedDLT(const Point2DVector& points2D, const Point3DVector& points3D)
{
unsigned int numberOfPoints = points2D.size();
if(points3D.size() != numberOfPoints)
{
std::stringstream ss;
ss << "ComputeP_NormalizedDLT: The number of 2D points (" << points2D.size()
<< ") must match the number of 3D points (" << points3D.size() << ")!" << std::endl;
throw std::runtime_error(ss.str());
}
// std::cout << "ComputeP_NormalizedDLT: 2D points: " << std::endl;
// for(Point2DVector::const_iterator iter = points2D.begin(); iter != points2D.end(); ++iter)
// {
// Point2DVector::value_type p = *iter;
// std::cout << p[0] << " " << p[1] << std::endl;
// }
// std::cout << "ComputeP_NormalizedDLT: 3D points: " << std::endl;
// for(Point3DVector::const_iterator iter = points3D.begin(); iter != points3D.end(); ++iter)
// {
// Point3DVector::value_type p = *iter;
// std::cout << p[0] << " " << p[1] << " " << p[2] << std::endl;
// }
Eigen::MatrixXd similarityTransform2D = ComputeNormalizationTransform<Eigen::Vector2d>(points2D);
Eigen::MatrixXd similarityTransform3D = ComputeNormalizationTransform<Eigen::Vector3d>(points3D);
// std::cout << "Computed similarity transforms:" << std::endl;
// std::cout << "similarityTransform2D: " << similarityTransform2D << std::endl;
// std::cout << "similarityTransform3D: " << similarityTransform3D << std::endl;
// The (, Eigen::VectorXd()) below are only required when using gnu++0x, it seems to be a bug in Eigen
Point2DVector transformed2DPoints(numberOfPoints, Eigen::Vector2d());
Point3DVector transformed3DPoints(numberOfPoints, Eigen::Vector3d());
for(unsigned int i = 0; i < numberOfPoints; ++i)
{
Eigen::VectorXd point2Dhomogeneous = points2D[i].homogeneous();
Eigen::VectorXd point2Dtransformed = similarityTransform2D * point2Dhomogeneous;
transformed2DPoints[i] = point2Dtransformed.hnormalized();
Eigen::VectorXd point3Dhomogeneous = points3D[i].homogeneous();
Eigen::VectorXd point3Dtransformed = similarityTransform3D * point3Dhomogeneous;
transformed3DPoints[i] = point3Dtransformed.hnormalized();
//transformed2DPoints[i] = (similarityTransform2D * points2D[i].homogeneous()).hnormalized();
//transformed3DPoints[i] = (similarityTransform3D * points3D[i].homogeneous()).hnormalized();
}
// std::cout << "Transformed points." << std::endl;
// Compute the Camera Projection Matrix
Eigen::MatrixXd A(2*numberOfPoints,12);
for(unsigned int i = 0; i < numberOfPoints; ++i)
{
// First row/equation from the ith correspondence
unsigned int row = 2*i;
A(row, 0) = 0;
A(row, 1) = 0;
A(row, 2) = 0;
A(row, 3) = 0;
A(row, 4) = transformed3DPoints[i](0);
A(row, 5) = transformed3DPoints[i](1);
A(row, 6) = transformed3DPoints[i](2);
A(row, 7) = 1;
A(row, 8) = -transformed2DPoints[i](1) * transformed3DPoints[i](0);
A(row, 9) = -transformed2DPoints[i](1) * transformed3DPoints[i](1);
A(row, 10) = -transformed2DPoints[i](1) * transformed3DPoints[i](2);
A(row, 11) = -transformed2DPoints[i](1);
// Second row/equation from the ith correspondence
row = 2*i+1;
A(row, 0) = transformed3DPoints[i](0);
A(row, 1) = transformed3DPoints[i](1);
A(row, 2) = transformed3DPoints[i](2);
A(row, 3) = 1;
A(row, 4) = 0;
A(row, 5) = 0;
A(row, 6) = 0;
A(row, 7) = 0;
A(row, 8) = -transformed2DPoints[i](0) * transformed3DPoints[i](0);
A(row, 9) = -transformed2DPoints[i](0) * transformed3DPoints[i](1);
A(row, 10) = -transformed2DPoints[i](0) * transformed3DPoints[i](2);
A(row, 11) = -transformed2DPoints[i](0);
}
// std::cout << "A: " << A << std::endl;
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::MatrixXd V = svd.matrixV();
Eigen::MatrixXd lastColumnOfV = V.col(11);
Eigen::MatrixXd P = Reshape(lastColumnOfV, 3, 4);
// Denormalization
P = similarityTransform2D.inverse()*P*similarityTransform3D; // 3x3 * 3x4 * 4x4 = 4x4
return P;
}
/**
* Generates a bathymetryGrid by reading data from a local file. Results are
* dumped into topographyGrid.
* @param topographyGrid A pointer to a zero-initialized Grid of size
* numRows x numCols.
* @param inputFile The relative path to the topography file you wish to use.
* @param inputFileType Use 'netcdf' if your file is in netcdf format. Use
* 'asc' if the file is a matrix of ascii values in the GIS asc format.
* @param startRow The row to start reading data from.
* @param startCol The col to start reading data from.
* @param numRows The desired number of rows of data to read.
* @param numCols The desired number of cols of data to read.
* @param seriesName If inputFileType was set to 'netcdf', this should be set
* to the name of the series you wish to use.
* @param timestamp A timestamp value used for error reporting.
*/
void getBathy(Grid* topographyGrid, std::string inputFile,
std::string inputFileType, size_t startRow, size_t startCol,
size_t numRows, size_t numCols, std::string seriesName,
std::string timestamp) {
// This will be the netCDF ID for the file and data variable.
Eigen::MatrixXd temp;
int ncid, varid, retval = -100;
// NetCDF
if (inputFileType.compare("netcdf") == 0) {
// Data will be read in column major, so set up a matrix of inverse
// size to recieve it.
temp.resize(numCols, numRows);
// Open the file. NC_NOWRITE tells netCDF we want read-only access to
// the file.
if ((retval = nc_open(inputFile.c_str(), NC_NOWRITE, &ncid))) {
printError("ERROR: Cant open NetCDF File. Invalid inputFile path.",
retval, timestamp);
}
// Get the varid of the data variable, based on its name.
if ((retval = nc_inq_varid(ncid, seriesName.c_str(), &varid))) {
printError("ERROR: Can't access variable id. Invalid seriesName.",
retval, timestamp);
}
// Read the data.
try {
// for whatever reason, this is in column, row order.
static size_t start[] = {startRow, startCol};
static size_t range[] = {numRows, numCols};
retval = nc_get_vara_double(ncid, varid, start, range,
temp.data());
// TODO(Greg) Figure out a way to read data in row wise to avoid
// this transposition.
topographyGrid->data.block(border, border, numRows, numCols) =
temp.transpose().block(0, 0, numRows, numCols);
}
catch (int i) {
printError("ERROR: Error reading data. Invalid file format.",
retval, timestamp);
}
// Close the file, freeing all resources.
if ((retval = nc_close(ncid))) {
printError("ERROR: Error closing the file.", retval, timestamp);
}
} else if (inputFileType.compare("asc") == 0) {
// ASC
temp.resize(numRows, numCols);
std::ifstream input(inputFile);
int null = 0;
size_t numHeaderLines = 5,
rowsLine = 1,
colsLine = 0,
nullLine = 5,
cursor = 0,
rows = 0,
cols = 0,
startRowIndex = startRow+numHeaderLines,
endRowIndex = startRowIndex+numRows,
i = 0,
j = 0;
std::string line = "";
std::vector<std::string> vLine;
if (input.is_open()) {
for (cursor = 0; cursor < endRowIndex; cursor ++) {
getline(input, line);
if (cursor <= numHeaderLines) {
// Get the number of columns in the file
if (cursor == colsLine) {
vLine = split(&line, ' ');
if (!silent) {
std::cout << "\nAvailable Cols: " <<
vLine[vLine.size()-1];
}
cols = stoi(vLine[vLine.size() - 1]);
if (cols < startCol + numCols) {
std::cout << "\nERROR, requested bathymetry " <<
" grid column coordinates are out of" <<
" bounds.\n";
exit(1);
}
} else if (cursor == rowsLine) {
// Get the number of rows in the file.
vLine = split(&line, ' ');
if (!silent) {
std::cout << "Available Rows:" <<
vLine[vLine.size() - 1];
}
rows = stoi(vLine[vLine.size() - 1]);
if (rows < startRow + numRows) {
std::cout << "\nERROR, requested bathymetry" <<
" grid row coordinates are out of" <<
" bounds.\n";
exit(1);
}
} else if (cursor == nullLine) {
// Get the null value substitute
vLine = split(&line, ' ');
if (debug) {
std::cout << "Null values =" <<
vLine[vLine.size() - 1] << "\n";
}
null = stoi(vLine[vLine.size() - 1]);
}
} else if (cursor >= startRowIndex) {
vLine = split(&line, ' ');
for (i = startCol; i < startCol + numCols; i ++) {
// std::cout<<"accessing temp(" <<
// cursor-startRowIndex-1 << "," <<i-startCol<<")\n";
// std::cout<<"Cursor:"<<cursor<<" SRI:" <<
// startRowIndex << "\n";
temp(cursor - startRowIndex, i - startCol) =
stod(vLine[i]);
}
}
}
input.close();
for (i = 0; i < numRows; i ++) {
for (j = 0; j < numCols; j ++) {
if (temp(i, j) == null) {
temp(i, j) = 0;
}
}
}
topographyGrid->data.block(border, border, numRows, numCols) =
temp.block(0, 0, numRows, numCols);
} else {
std::cout << "\nUnable to open bathymetry file: \"" << inputFile <<
"\"\n";
exit(0);
}
} else {
// Invalid filetype
std::cout << "Bathymetry file type not supported. Simulating" <<
"Bathymetry.\n";
simulatetopographyGrid(topographyGrid, static_cast<int>(numRows),
static_cast<int>(numCols));
}
topographyGrid->clearNA();
topographyGrid->data =
topographyGrid->data.unaryExpr(std::ptr_fun(validateDepth));
if (acousticParams["debug"] == "1") {
// topographyGrid->printData();
std::cout << "startx " << startCol << "\nXDist: "<< numCols <<
"\nstartY: "<< startRow << "\nYDist: " << numRows << "\n";
std::cout << "inputFileType: " << inputFileType << "\ninputFile: " <<
inputFile << "\nseriesName: " << seriesName << "\n";
std::cout << "retval: " << retval << "\n" << "ncid: " << ncid << "\n\n";
}
}
void display()
{
using namespace Eigen;
using namespace igl;
using namespace std;
glClearColor(back[0],back[1],back[2],0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// All smooth points
glEnable( GL_POINT_SMOOTH );
lights();
push_scene();
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glEnable(GL_NORMALIZE);
glEnable(GL_COLOR_MATERIAL);
glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
push_object();
if(trackball_on)
{
// Draw a "laser" line
glLineWidth(3.0);
glDisable(GL_LIGHTING);
glEnable(GL_DEPTH_TEST);
glBegin(GL_LINES);
glColor3f(1,0,0);
glVertex3fv(s.data());
glColor3f(1,0,0);
glVertex3fv(d.data());
glEnd();
// Draw the start and end points used for ray
glPointSize(10.0);
glBegin(GL_POINTS);
glColor3f(1,0,0);
glVertex3fv(s.data());
glColor3f(0,0,1);
glVertex3fv(d.data());
glEnd();
}
// Draw the model
glEnable(GL_LIGHTING);
draw_mesh(V,F,N,C);
// Draw all hits
glBegin(GL_POINTS);
glColor3f(0,0.2,0.2);
for(vector<igl::embree::Hit>::iterator hit = hits.begin();
hit != hits.end();
hit++)
{
const double w0 = (1.0-hit->u-hit->v);
const double w1 = hit->u;
const double w2 = hit->v;
VectorXd hitP =
w0 * V.row(F(hit->id,0)) +
w1 * V.row(F(hit->id,1)) +
w2 * V.row(F(hit->id,2));
glVertex3dv(hitP.data());
}
glEnd();
pop_object();
// Draw a nice floor
glPushMatrix();
glEnable(GL_LIGHTING);
glTranslated(0,-1,0);
draw_floor();
glPopMatrix();
// draw a transparent "projection screen" show model at time of hit (aka
// mouse down)
push_object();
if(trackball_on)
{
glColor4f(0,0,0,1.0);
glPointSize(10.0);
glBegin(GL_POINTS);
glVertex3fv(SW.data());
glVertex3fv(SE.data());
glVertex3fv(NE.data());
glVertex3fv(NW.data());
glEnd();
glDisable(GL_LIGHTING);
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, tex_id);
glColor4f(1,1,1,0.7);
glBegin(GL_QUADS);
glTexCoord2d(0,0);
glVertex3fv(SW.data());
glTexCoord2d(1,0);
glVertex3fv(SE.data());
glTexCoord2d(1,1);
glVertex3fv(NE.data());
glTexCoord2d(0,1);
glVertex3fv(NW.data());
glEnd();
glBindTexture(GL_TEXTURE_2D, 0);
glDisable(GL_TEXTURE_2D);
}
pop_object();
pop_scene();
// Draw a faint point over mouse
if(!trackball_on)
{
glDisable(GL_LIGHTING);
glDisable(GL_COLOR_MATERIAL);
glDisable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor4f(1.0,0.3,0.3,0.6);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluOrtho2D(0,width,0,height);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glPointSize(20.0);
glBegin(GL_POINTS);
glVertex2fv(win_s.data());
glEnd();
}
report_gl_error();
glutSwapBuffers();
glutPostRedisplay();
}
/**
* Generates an artificial topographyGrid of size numRows x numCols if no
* topographic data is available. Results are dumped into topographyGrid.
* @param topographyGrid A pointer to a zero-initialized Grid of size
* numRows x numCols.
* @param numRows The desired number of non-border rows in the resulting matrix.
* @param numCols The desired number of non-border cols in the resulting matrix.
*/
void simulatetopographyGrid(Grid* topographyGrid, int numRows, int numCols) {
Eigen::VectorXd refx = refx.LinSpaced(numCols, -2*M_PI, 2*M_PI);
Eigen::VectorXd refy = refx.LinSpaced(numRows, -2*M_PI, 2*M_PI);
Eigen::MatrixXd X = refx.replicate(1, numRows);
X.transposeInPlace();
Eigen::MatrixXd Y = refy.replicate(1, numCols);
// Eigen can only deal with two matrices at a time,
// so split the computation:
// topographyGrid = sin(X) * sin(Y) * abs(X) * abs(Y) -pi
Eigen::MatrixXd absXY = X.cwiseAbs().cwiseProduct(Y.cwiseAbs());
Eigen::MatrixXd sins = X.array().sin().cwiseProduct(Y.array().sin());
Eigen::MatrixXd temp;
temp.resize(numRows, numCols);
temp = absXY.cwiseProduct(sins);
// All this work to create a matrix of pi...
Eigen::MatrixXd pi;
pi.resize(numRows, numCols);
pi.setConstant(M_PI);
temp = temp - pi;
// And the final result.
topographyGrid->data.block(border, border, numRows, numCols) =
temp.block(0, 0, numRows, numCols);
// Ignore positive values.
topographyGrid->data =
topographyGrid->data.unaryExpr(std::ptr_fun(validateDepth));
topographyGrid->clearNA();
}
void CDKF::measurementUpdate(const ros::Time& prev_stamp,
const ros::Time& current_stamp,
const double image_angular_velocity,
const IMUList& imu_rotations,
const bool calc_offset) {
// convert tracked points to measurement
Eigen::VectorXd real_measurement(kMeasurementSize);
accessM(real_measurement, MEASURED_TIMESTAMP).array() =
(current_stamp - zero_timestamp_).toSec();
accessM(real_measurement, ANGULAR_VELOCITY).array() = image_angular_velocity;
if (verbose_) {
ROS_INFO_STREAM("Measured Values: \n" << real_measurement.transpose());
}
// create sigma points
MeasurementSigmaPoints sigma_points(
state_, cov_, measurement_noise_sd_, CDKF::stateToMeasurementEstimate,
imu_rotations, zero_timestamp_, calc_offset);
// get mean and cov
Eigen::VectorXd predicted_measurement;
sigma_points.calcEstimatedMean(&predicted_measurement);
Eigen::MatrixXd innovation;
sigma_points.calcEstimatedCov(&innovation);
if (verbose_) {
ROS_INFO_STREAM("Predicted Measurements: \n"
<< predicted_measurement.transpose());
}
// calc mah distance
Eigen::VectorXd diff = real_measurement - predicted_measurement;
double mah_dist = std::sqrt(diff.transpose() * innovation.inverse() * diff);
if (mah_dist > mah_threshold_) {
ROS_WARN("Outlier detected, measurement rejected");
return;
}
Eigen::MatrixXd cross_cov;
sigma_points.calcEstimatedCrossCov(&cross_cov);
Eigen::MatrixXd gain = cross_cov * innovation.inverse();
const Eigen::VectorXd state_diff =
gain * (real_measurement - predicted_measurement);
state_ += state_diff;
cov_ -= gain * innovation * gain.transpose();
if (verbose_) {
ROS_INFO_STREAM("Updated State: \n" << state_.transpose());
ROS_INFO_STREAM("Updated Cov: \n" << cov_);
}
// guard against precision issues
constexpr double kMaxTime = 10000.0;
if (accessS(state_, STATE_TIMESTAMP)[0] > kMaxTime) {
rezeroTimestamps(current_stamp);
}
}
void display()
{
if(sorted_F.size() != F.size())
{
mouse(0,1,-1,-1);
}
//using namespace Eigen;
using namespace igl;
using namespace std;
glClearColor(BG[0],BG[1],BG[2],0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// All smooth points
glEnable( GL_POINT_SMOOTH );
lights();
push_scene();
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glEnable(GL_NORMALIZE);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
//glEnable(GL_CULL_FACE);
//glCullFace(GL_BACK);
// Draw a nice floor
push_object();
glEnable(GL_LIGHTING);
glTranslated(0,V.col(1).minCoeff(),0);
glScaled(2*bbd,2*bbd,2*bbd);
glTranslated(0,-1000*FLOAT_EPS,0);
glEnable(GL_CULL_FACE);
glCullFace(GL_BACK);
igl::opengl2::draw_floor();
pop_object();
glDisable(GL_CULL_FACE);
if(trackball_on)
{
glEnable(GL_COLOR_MATERIAL);
glDisable(GL_LIGHTING);
}else
{
float front[4];
copy(GOLD_DIFFUSE,GOLD_DIFFUSE+3,front);
float back[4];
//copy(FAST_GREEN_DIFFUSE,FAST_GREEN_DIFFUSE+3,back);
copy(GOLD_DIFFUSE,GOLD_DIFFUSE+3,back);
float amb[4];
copy(SILVER_AMBIENT,SILVER_AMBIENT+3,amb);
float spec[4];
copy(SILVER_SPECULAR,SILVER_SPECULAR+3,spec);
front[3] = back[3] = amb[3] = spec[3] = alpha;
// Set material properties
glDisable(GL_COLOR_MATERIAL);
glMaterialfv(GL_FRONT, GL_AMBIENT, amb);
glMaterialfv(GL_FRONT, GL_DIFFUSE, front);
glMaterialfv(GL_FRONT, GL_SPECULAR, spec);
glMaterialf (GL_FRONT, GL_SHININESS, 128);
glMaterialfv(GL_BACK, GL_AMBIENT, amb);
glMaterialfv(GL_BACK, GL_DIFFUSE, back);
glMaterialfv(GL_BACK, GL_SPECULAR, spec);
glMaterialf (GL_BACK, GL_SHININESS, 128);
glEnable(GL_LIGHTING);
}
push_object();
// Draw the model
igl::opengl2::draw_mesh(V,sorted_F,sorted_N,C);
pop_object();
pop_scene();
igl::opengl::report_gl_error();
glutSwapBuffers();
glutPostRedisplay();
}
TEST_F(ExoticaTaskTest, DTT) //!< Derived-task Jacobian Tests: will contain tests to check whether the jacobian is correctly computed through finite-differences method
{
boost::shared_ptr<exotica::TaskDefinition> task_ptr;
for (int i = 0; i < registered_types_.size(); i++) //!< Iterate through the registered tasks
{
std::string xml_path;
tinyxml2::XMLDocument document;
boost::shared_ptr<tinyxml2::XMLHandle> element_ptr;
if (exotica::TestRegistrar::Instance()->findXML(registered_types_[i],
xml_path)) //!< If it is wished to be tested...
{
if (document.LoadFile((resource_path_ + xml_path).c_str())
!= tinyxml2::XML_NO_ERROR) //!< Attempt to load file
{
ADD_FAILURE()<< " : Could not Load initialiser for " << registered_types_[i] << " (file: "<<resource_path_ + xml_path <<")."; //!< Have to use this method to add failure since I do not want to continue for this object but do not wish to abort for all the others...
continue;//!< Go to next object
}
//!< Initialise
if (!(task_ptr = exotica::TaskCreator::Instance()->createObject(registered_types_[i], params_)))//!< If we could not create
{
ADD_FAILURE() << " : Could not create object of type " << registered_types_[i]; //!< Have to use this method to add failure since I do not want to continue for this object but do not wish to abort for all the others...
continue;//!< Go to next object
}
tinyxml2::XMLHandle xml_handle(document.RootElement()); //!< Get a handle to the root element
if (!task_ptr->initBase(xml_handle))
{
ADD_FAILURE() << " : XML Initialiser is malformed for " << registered_types_[i];
continue;
}
//!< Now our testing stuff...
element_ptr.reset(new tinyxml2::XMLHandle(xml_handle.FirstChildElement("TestPoint")));
if (!element_ptr)
{
ADD_FAILURE() << " : XML Tester is malformed for " << registered_types_[i];
continue;
}
int j=0;
while (element_ptr->ToElement()) //!< Iterate through all possible test cases
{
Eigen::VectorXd conf_space; //!< Vector containing the configuration point for testing
Eigen::VectorXd task_space;//!< Vector with the task-space co-ordinate of the forward map
Eigen::VectorXd phi;//!< The computed phi
Eigen::MatrixXd jacobian;//!< The Jacobian computation
if (!element_ptr->FirstChildElement("ConfSpace").ToElement())//!< If inexistent
{
ADD_FAILURE() << " : Missing Config for " << registered_types_[i] << " @ iteration " << j;
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
continue; //!< With the inner while loop...
}
if (!exotica::getVector(*(element_ptr->FirstChildElement("ConfSpace").ToElement()), conf_space))
{
ADD_FAILURE() << " : Could not parse Config Vector for " << registered_types_[i] << " @ iteration " << j;
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
continue; //!< With the inner while loop...
}
if (!element_ptr->FirstChildElement("TaskSpace").ToElement()) //!< If inexistent
{
ADD_FAILURE() << " : Missing Task Space point for " << registered_types_[i] << " @ iteration " << j;
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
continue; //!< With the inner while loop...
}
if (!exotica::getVector(*(element_ptr->FirstChildElement("TaskSpace").ToElement()), task_space))
{
ADD_FAILURE() << " : Could not parse task vector for " << registered_types_[i] << " @ iteration " << j;
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
continue; //!< With the inner while loop...
}
//!< First test forward mapping
if (!task_ptr->updateTask(conf_space, 0))
{
ADD_FAILURE() << " for " << registered_types_[i] << " @ iteration " << j;
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
continue;
}
phi = task_ptr->getPhi(0);
jacobian = task_ptr->getJacobian(0);
if (phi.size() != task_space.size())
{
ADD_FAILURE() << " Task-size mismatch for " << registered_types_[i] << " @ iteration " << j;
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
continue;
}
EXPECT_TRUE(compareVectors(phi, task_space, TOLERANCE)) << " : Phi mismatch for " << registered_types_[i] << " @ iteration " << j;
//!< Now the Jacobian (finite differences method)
for (int k=0; k<conf_space.size(); k++)
{
Eigen::VectorXd conf_perturb = conf_space;
conf_perturb(k) += EPSILON;
if (!task_ptr->updateTask(conf_perturb, 0))
{
ADD_FAILURE() << " for " << registered_types_[i] << " @ iteration " << j << " in config-dimension " << k;
continue;
}
if (task_ptr->getPhi(0).size() != phi.size())
{
ADD_FAILURE() << " for " << registered_types_[i] << " @ iteration " << j << " in config-dimension " << k;
continue;
}
Eigen::VectorXd task_diff = (task_ptr->getPhi(0) - phi)/EPSILON; //!< Final - initial
Eigen::VectorXd jac_column = jacobian.col(k);//!< Get the desired column (for the current joint)
EXPECT_TRUE(compareVectors(task_diff, jac_column, TOLERANCE)) << " Incorrect for " << registered_types_[i] << " @ iteration " << j << " in config-dimension " << k << task_diff << "\n" << jac_column;
}
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
} //!< Iteration through j
} //check on testing
} //!< Iteration through i
}
// computes the optimal rigid body transformation given a set of points
void PoseTracker::computeOptimalRigidTransformation(Eigen::MatrixXd startP, Eigen::MatrixXd finalP)
{
Eigen::Matrix4d transf;
if (startP.rows()!=finalP.rows())
{
ROS_ERROR("We need that the rows be the same at the beggining");
exit(1);
}
Eigen::RowVector3d centroid_startP = Eigen::RowVector3d::Zero();
Eigen::RowVector3d centroid_finalP = Eigen::RowVector3d::Zero(); //= mean(B);
Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
//calculate the mean
for (int i=0;i<startP.rows();i++)
{
centroid_startP = centroid_startP+startP.row(i);
centroid_finalP = centroid_finalP+finalP.row(i);
}
centroid_startP = centroid_startP/startP.rows();
centroid_finalP = centroid_finalP/startP.rows();
for (int i=0;i<startP.rows();i++)
H = H + (startP.row(i)-centroid_startP).transpose()*(finalP.row(i)-centroid_finalP);
Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::MatrixXd U = svd.matrixU();
Eigen::MatrixXd V = svd.matrixV();
if (V.determinant()<0)
V.col(2)=-V.col(2)*(-1);
Eigen::MatrixXd R = V*U.transpose();
Eigen::Matrix4d C_A = Eigen::Matrix4d::Identity();
Eigen::Matrix4d C_B = Eigen::Matrix4d::Identity();
Eigen::Matrix4d R_new = Eigen::Matrix4d::Identity();
C_A.block<3,1>(0,3) = -centroid_startP.transpose();
R_new.block<3,3>(0,0) = R;
C_B.block<3,1>(0,3) = centroid_finalP.transpose();
transf = C_B * R_new * C_A;
Eigen::Quaterniond mat_rot(transf.block<3,3>(0,0));
Eigen::Vector3d trasl = transf.block<3,1>(0,3).transpose();
transfParameters_ << trasl(0), trasl(1), trasl(2), mat_rot.x(), mat_rot.y(), mat_rot.z(), mat_rot.w();
}
void display()
{
using namespace igl;
using namespace std;
using namespace Eigen;
const float back[4] = {0.75, 0.75, 0.75,0};
glClearColor(back[0],back[1],back[2],0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
static bool first = true;
if(first)
{
sort();
first = false;
}
if(is_animating)
{
double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION;
if(t > 1)
{
t = 1;
is_animating = false;
}
Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized();
camera.orbit(q.conjugate());
}
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glEnable(GL_NORMALIZE);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
lights();
push_scene();
// Draw a nice floor
glEnable(GL_DEPTH_TEST);
glPushMatrix();
const double floor_offset =
-2./bbd*(V.col(1).maxCoeff()-Vmid(1));
glTranslated(0,floor_offset,0);
const float GREY[4] = {0.5,0.5,0.6,1.0};
const float DARK_GREY[4] = {0.2,0.2,0.3,1.0};
glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
glEnable(GL_CULL_FACE);
glCullFace(GL_BACK);
draw_floor(GREY,DARK_GREY);
glDisable(GL_CULL_FACE);
glPopMatrix();
push_object();
const auto & draw_skeleton = []()
{
switch(skel_style)
{
default:
case SKEL_STYLE_TYPE_3D:
{
MatrixXf colors = MAYA_VIOLET.transpose().replicate(s.BE.rows(),1);
for(int si=0;si<s.sel.size();si++)
{
for(int b=0;b<s.BE.rows();b++)
{
if(s.BE(b,0) == s.sel(si) || s.BE(b,1) == s.sel(si))
{
colors.row(b) = MAYA_SEA_GREEN;
}
}
}
draw_skeleton_3d(s.C,s.BE,MatrixXd(),colors);
break;
}
case SKEL_STYLE_TYPE_VECTOR_GRAPHICS:
draw_skeleton_vector_graphics(s.C,s.BE);
break;
}
};
if(!skeleton_on_top)
{
draw_skeleton();
}
// Set material properties
glDisable(GL_COLOR_MATERIAL);
glMaterialfv(GL_FRONT, GL_AMBIENT,
Vector4f(GOLD_AMBIENT[0],GOLD_AMBIENT[1],GOLD_AMBIENT[2],alpha).data());
glMaterialfv(GL_FRONT, GL_DIFFUSE,
Vector4f(GOLD_DIFFUSE[0],GOLD_DIFFUSE[1],GOLD_DIFFUSE[2],alpha).data());
glMaterialfv(GL_FRONT, GL_SPECULAR,
Vector4f(GOLD_SPECULAR[0],GOLD_SPECULAR[1],GOLD_SPECULAR[2],alpha).data());
glMaterialf (GL_FRONT, GL_SHININESS, 128);
glMaterialfv(GL_BACK, GL_AMBIENT,
Vector4f(SILVER_AMBIENT[0],SILVER_AMBIENT[1],SILVER_AMBIENT[2],alpha).data());
glMaterialfv(GL_BACK, GL_DIFFUSE,
Vector4f(FAST_GREEN_DIFFUSE[0],FAST_GREEN_DIFFUSE[1],FAST_GREEN_DIFFUSE[2],alpha).data());
glMaterialfv(GL_BACK, GL_SPECULAR,
Vector4f(SILVER_SPECULAR[0],SILVER_SPECULAR[1],SILVER_SPECULAR[2],alpha).data());
glMaterialf (GL_BACK, GL_SHININESS, 128);
if(wireframe)
{
glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
}
glLineWidth(1.0);
draw_mesh(V,sorted_F,sorted_N);
glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
if(skeleton_on_top)
{
glDisable(GL_DEPTH_TEST);
draw_skeleton();
}
pop_object();
pop_scene();
report_gl_error();
TwDraw();
glutSwapBuffers();
if(is_animating)
{
glutPostRedisplay();
}
}
RcppExport SEXP halfTransform (SEXP _transform)
{
BEGIN_RCPP
RObject transform(_transform);
RObject result;
if (transform.inherits("affine"))
{
Eigen::MatrixXd matrix = as<Eigen::MatrixXd>(_transform);
matrix = (matrix.log() * 0.5).exp();
result = AffineMatrix(matrix);
}
else
{
NiftiImage transformationImage(_transform);
switch (reg_round(transformationImage->intent_p1))
{
case SPLINE_GRID:
reg_getDisplacementFromDeformation(transformationImage);
reg_tools_multiplyValueToImage(transformationImage,transformationImage,0.5f);
reg_getDeformationFromDisplacement(transformationImage);
break;
case DEF_FIELD:
reg_getDisplacementFromDeformation(transformationImage);
reg_tools_multiplyValueToImage(transformationImage,transformationImage,0.5f);
reg_getDeformationFromDisplacement(transformationImage);
break;
case DISP_FIELD:
reg_tools_multiplyValueToImage(transformationImage,transformationImage,0.5f);
break;
case SPLINE_VEL_GRID:
reg_getDisplacementFromDeformation(transformationImage);
reg_tools_multiplyValueToImage(transformationImage,transformationImage,0.5f);
reg_getDeformationFromDisplacement(transformationImage);
--transformationImage->intent_p2;
if (transformationImage->num_ext>1)
--transformationImage->num_ext;
break;
case DEF_VEL_FIELD:
reg_getDisplacementFromDeformation(transformationImage);
reg_tools_multiplyValueToImage(transformationImage,transformationImage,0.5f);
reg_getDeformationFromDisplacement(transformationImage);
--transformationImage->intent_p2;
break;
case DISP_VEL_FIELD:
reg_tools_multiplyValueToImage(transformationImage,transformationImage,0.5f);
--transformationImage->intent_p2;
break;
default:
throw std::runtime_error("The specified transformation image is not valid or not supported");
}
result = transformationImage.toPointer("F3D transformation");
}
result.attr("source") = transform.attr("source");
result.attr("target") = transform.attr("target");
return result;
END_RCPP
}
/**
* Makes a Grid out of an existing Eigen matrix.
* @param dat An existing eigen matrix.
* @param newName The name of the newly created Grid. Used when writing files.
*/
Grid::Grid(Eigen::MatrixXd dat, std::string newName) {
rows = dat.rows();
cols = dat.cols();
name = newName;
data = dat;
}
Eigen::MatrixXd padMatrix(Eigen::MatrixXd d, int r)
{
using namespace Eigen;
MatrixXd out = MatrixXd::Zero(d.rows()+2*r, d.cols()+2*r);
out.block(r, r, d.rows(), d.cols()) = d;
out.block(r, 0, d.rows(), r) =
d.block(0, 0, d.rows(), r).rowwise().reverse();
out.block(r, d.cols()+r, d.rows(), r) =
d.block(0, d.cols()-r, d.rows(), r).rowwise().reverse();
out.block(0, 0, r, out.cols()) =
out.block(r, 0, r, out.cols()).colwise().reverse();
out.block(d.rows()+r, 0, r, out.cols()) =
out.block(out.rows()-r, 0, r, out.cols()).colwise().reverse();
return out;
}
void propa_2d()
{
trace.beginBlock("2d propagation");
typedef DiscreteExteriorCalculus<2, 2, EigenLinearAlgebraBackend> Calculus;
typedef DiscreteExteriorCalculusFactory<EigenLinearAlgebraBackend> CalculusFactory;
{
trace.beginBlock("solving time dependent equation");
const Calculus::Scalar cc = 8; // px/s
trace.info() << "cc = " << cc << endl;
const Z2i::Domain domain(Z2i::Point(0,0), Z2i::Point(29,29));
const Calculus calculus = CalculusFactory::createFromDigitalSet(generateDiskSet(domain), false);
//! [time_laplace]
const Calculus::DualIdentity0 laplace = calculus.laplace<DUAL>() + 1e-8 * calculus.identity<0, DUAL>();
//! [time_laplace]
trace.info() << "laplace = " << laplace << endl;
trace.beginBlock("finding eigen pairs");
//! [time_eigen]
typedef Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> EigenSolverMatrix;
const EigenSolverMatrix eigen_solver(laplace.myContainer);
const Eigen::VectorXd eigen_values = eigen_solver.eigenvalues();
const Eigen::MatrixXd eigen_vectors = eigen_solver.eigenvectors();
//! [time_eigen]
trace.info() << "eigen_values_range = " << eigen_values.minCoeff() << "/" << eigen_values.maxCoeff() << endl;
trace.endBlock();
//! [time_omega]
const Eigen::VectorXd angular_frequencies = cc * eigen_values.array().sqrt();
//! [time_omega]
Eigen::VectorXcd initial_wave = Eigen::VectorXcd::Zero(calculus.kFormLength(0, DUAL));
//set_initial_phase_null(calculus, initial_wave);
set_initial_phase_dir_yy(calculus, initial_wave);
{
Board2D board;
board << domain;
board << CustomStyle("KForm", new KFormStyle2D(-1, 1));
board << Calculus::DualForm0(calculus, initial_wave.real());
board.saveSVG("propagation_time_wave_initial_coarse.svg");
}
//! [time_init_proj]
Eigen::VectorXcd initial_projections = eigen_vectors.transpose() * initial_wave;
//! [time_init_proj]
// low pass
//! [time_low_pass]
const Calculus::Scalar lambda_cutoff = 4.5;
const Calculus::Scalar angular_frequency_cutoff = 2*M_PI * cc / lambda_cutoff;
int cutted = 0;
for (int kk=0; kk<initial_projections.rows(); kk++)
{
const Calculus::Scalar angular_frequency = angular_frequencies(kk);
if (angular_frequency < angular_frequency_cutoff) continue;
initial_projections(kk) = 0;
cutted ++;
}
//! [time_low_pass]
trace.info() << "cutted = " << cutted << "/" << initial_projections.rows() << endl;
{
const Eigen::VectorXcd wave = eigen_vectors * initial_projections;
Board2D board;
board << domain;
board << CustomStyle("KForm", new KFormStyle2D(-1, 1));
board << Calculus::DualForm0(calculus, wave.real());
board.saveSVG("propagation_time_wave_initial_smoothed.svg");
}
const int kk_max = 60;
trace.progressBar(0,kk_max);
for (int kk=0; kk<kk_max; kk++)
{
//! [time_solve_time]
const Calculus::Scalar time = kk/20.;
const Eigen::VectorXcd current_projections = (angular_frequencies * std::complex<double>(0,time)).array().exp() * initial_projections.array();
const Eigen::VectorXcd current_wave = eigen_vectors * current_projections;
//! [time_solve_time]
std::stringstream ss;
ss << "propagation_time_wave_solution_" << std::setfill('0') << std::setw(3) << kk << ".svg";
Board2D board;
board << domain;
board << CustomStyle("KForm", new KFormStyle2D(-1, 1));
board << Calculus::DualForm0(calculus, current_wave.real());
board.saveSVG(ss.str().c_str());
trace.progressBar(kk+1,kk_max);
}
trace.info() << endl;
trace.endBlock();
}
{
trace.beginBlock("forced oscillations");
const Z2i::Domain domain(Z2i::Point(0,0), Z2i::Point(50,50));
const Calculus calculus = CalculusFactory::createFromDigitalSet(generateDiskSet(domain), false);
const Calculus::DualIdentity0 laplace = calculus.laplace<DUAL>();
trace.info() << "laplace = " << laplace << endl;
trace.beginBlock("finding eigen pairs");
typedef Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> EigenSolverMatrix;
const EigenSolverMatrix eigen_solver(laplace.myContainer);
const Eigen::VectorXd laplace_eigen_values = eigen_solver.eigenvalues();
const Eigen::MatrixXd laplace_eigen_vectors = eigen_solver.eigenvectors();
trace.info() << "eigen_values_range = " << laplace_eigen_values.minCoeff() << "/" << laplace_eigen_values.maxCoeff() << endl;
trace.endBlock();
Calculus::DualForm0 concentration(calculus);
{
const Z2i::Point point(25,25);
const Calculus::Cell cell = calculus.myKSpace.uSpel(point);
const Calculus::Index index = calculus.getCellIndex(cell);
concentration.myContainer(index) = 1;
}
{
Board2D board;
board << domain;
board << concentration;
board.saveSVG("propagation_forced_concentration.svg");
}
for (int ll=0; ll<6; ll++)
{
//! [forced_lambda]
const Calculus::Scalar lambda = 4*20.75/(1+2*ll);
//! [forced_lambda]
trace.info() << "lambda = " << lambda << endl;
//! [forced_dalembert_eigen]
const Eigen::VectorXd dalembert_eigen_values = (laplace_eigen_values.array() - (2*M_PI/lambda)*(2*M_PI/lambda)).array().inverse();
const Eigen::MatrixXd concentration_to_wave = laplace_eigen_vectors * dalembert_eigen_values.asDiagonal() * laplace_eigen_vectors.transpose();
//! [forced_dalembert_eigen]
//! [forced_wave]
Calculus::DualForm0 wave(calculus, concentration_to_wave * concentration.myContainer);
//! [forced_wave]
wave.myContainer /= wave.myContainer(calculus.getCellIndex(calculus.myKSpace.uSpel(Z2i::Point(25,25))));
{
trace.info() << "saving samples" << endl;
const Calculus::Properties properties = calculus.getProperties();
{
std::stringstream ss;
ss << "propagation_forced_samples_hv_" << ll << ".dat";
std::ofstream handle(ss.str().c_str());
for (int kk=0; kk<=50; kk++)
{
const Z2i::Point point_horizontal(kk,25);
const Z2i::Point point_vertical(25,kk);
const Calculus::Scalar xx = 2 * (kk/50. - .5);
handle << xx << " ";
handle << sample_dual_0_form<Calculus>(properties, wave, point_horizontal) << " ";
handle << sample_dual_0_form<Calculus>(properties, wave, point_vertical) << endl;
}
}
{
std::stringstream ss;
ss << "propagation_forced_samples_diag_" << ll << ".dat";
std::ofstream handle(ss.str().c_str());
for (int kk=0; kk<=50; kk++)
{
const Z2i::Point point_diag_pos(kk,kk);
const Z2i::Point point_diag_neg(kk,50-kk);
const Calculus::Scalar xx = 2 * sqrt(2) * (kk/50. - .5);
handle << xx << " ";
handle << sample_dual_0_form<Calculus>(properties, wave, point_diag_pos) << " ";
handle << sample_dual_0_form<Calculus>(properties, wave, point_diag_neg) << endl;
}
}
}
{
std::stringstream ss;
ss << "propagation_forced_wave_" << ll << ".svg";
Board2D board;
board << domain;
board << CustomStyle("KForm", new KFormStyle2D(-.5, 1));
board << wave;
board.saveSVG(ss.str().c_str());
}
}
trace.endBlock();
}
trace.endBlock();
}
virtual void operate() {
time_vector[0] = time_vector[1];
time_vector[1] = this->time.getValue();
del_time = time_vector[0] - time_vector[1];
for (j = 0; j < mode - 1; j++) {
for (i = 0; i < (int) DOF; i++) {
container(j, i) = container(j + 1, i);
}
}
for (i = 0; i < (int) DOF; i++) // updating the last entry
{
container(mode - 1, i) = this->inputSignal.getValue()[i];
}
if (mode == 5) // for mode 2
{
diff_output = (-2.0 * container.block(2, 0, 1, (int) DOF)
+ container.block(0, 0, 1, (int) DOF)
+ container.block(4, 0, 1, (int) DOF))
/ (4.0 * del_time * del_time);
}
if (mode == 7) // for mode 3
{
diff_output = (-4.0 * container.block(3, 0, 1, (int) DOF)
- (container.block(2, 0, 1, (int) DOF)
+ container.block(4, 0, 1, (int) DOF))
+ 2.0
* (container.block(1, 0, 1, (int) DOF)
+ container.block(5, 0, 1, (int) DOF))
+ (container.block(0, 0, 1, (int) DOF)
+ container.block(6, 0, 1, (int) DOF)))
/ (16.0 * del_time * del_time);
}
if (mode == 9) // for mode 3
{
diff_output = (-10.0 * container.block(4, 0, 1, (int) DOF)
- 4.0
* (container.block(3, 0, 1, (int) DOF)
+ container.block(5, 0, 1, (int) DOF))
+ 4.0
* (container.block(2, 0, 1, (int) DOF)
+ container.block(6, 0, 1, (int) DOF))
+ 4.0
* (container.block(1, 0, 1, (int) DOF)
+ container.block(7, 0, 1, (int) DOF))
+ 1.0
* (container.block(0, 0, 1, (int) DOF)
+ container.block(8, 0, 1, (int) DOF)))
/ (64.0 * del_time * del_time);
}
for (i = 1; i < (int) DOF; i++) {
tmp_out_value[i] = diff_output[i];
}
this->outputSignalOutputValue->setData(&tmp_out_value);
}
void BranchList::findBranchesInCenterline(Eigen::MatrixXd positions)
{
positions = sortMatrix(2,positions);
Eigen::MatrixXd positionsNotUsed = positions;
// int minIndex;
int index;
int splitIndex;
Eigen::MatrixXd::Index startIndex;
BranchPtr branchToSplit;
while (positionsNotUsed.cols() > 0)
{
if (!mBranches.empty())
{
double minDistance = 1000;
for (int i = 0; i < mBranches.size(); i++)
{
std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distances;
distances = dsearchn(positionsNotUsed, mBranches[i]->getPositions());
double d = distances.second.minCoeff(&index);
if (d < minDistance)
{
minDistance = d;
branchToSplit = mBranches[i];
startIndex = index;
if (minDistance < 2)
break;
}
}
std::pair<Eigen::MatrixXd::Index, double> dsearchResult = dsearch(positionsNotUsed.col(startIndex) , branchToSplit->getPositions());
splitIndex = dsearchResult.first;
}
else //if this is the first branch. Select the top position (Trachea).
startIndex = positionsNotUsed.cols() - 1;
std::pair<Eigen::MatrixXd,Eigen::MatrixXd > connectedPointsResult = findConnectedPointsInCT(startIndex , positionsNotUsed);
Eigen::MatrixXd branchPositions = connectedPointsResult.first;
positionsNotUsed = connectedPointsResult.second;
if (branchPositions.cols() >= 5) //only include brances of length >= 5 points
{
BranchPtr newBranch = BranchPtr(new Branch());
newBranch->setPositions(branchPositions);
mBranches.push_back(newBranch);
if (mBranches.size() > 1) // do not try to split another branch when the first branch is processed
{
if ((splitIndex + 1 >= 5) && (branchToSplit->getPositions().cols() - splitIndex - 1 >= 5))
//do not split branch if the new branch is close to the edge of the branch
//if the new branch is not close to one of the edges of the
//connected existing branch: Split the existing branch
{
BranchPtr newBranchFromSplit = BranchPtr(new Branch());
Eigen::MatrixXd branchToSplitPositions = branchToSplit->getPositions();
newBranchFromSplit->setPositions(branchToSplitPositions.rightCols(branchToSplitPositions.cols() - splitIndex - 1));
branchToSplit->setPositions(branchToSplitPositions.leftCols(splitIndex + 1));
mBranches.push_back(newBranchFromSplit);
newBranchFromSplit->setParentBranch(branchToSplit);
newBranch->setParentBranch(branchToSplit);
newBranchFromSplit->setChildBranches(branchToSplit->getChildBranches());
branchVector branchToSplitChildren = branchToSplit->getChildBranches();
for (int i = 0; i < branchToSplitChildren.size(); i++)
branchToSplitChildren[i]->setParentBranch(newBranchFromSplit);
branchToSplit->deleteChildBranches();
branchToSplit->addChildBranch(newBranchFromSplit);
branchToSplit->addChildBranch(newBranch);
}
else if (splitIndex + 1 < 5)
// If the new branch is close to the start of the existing
// branch: Connect it to the same position start as the
// existing branch
{
newBranch->setParentBranch(branchToSplit->getParentBranch());
branchToSplit->getParentBranch()->addChildBranch(newBranch);
}
else if (branchToSplit->getPositions().cols() - splitIndex - 1 < 5)
// If the new branch is close to the end of the existing
// branch: Connect it to the end of the existing branch
{
newBranch->setParentBranch(branchToSplit);
branchToSplit->addChildBranch(newBranch);
}
}
}
}
}
void run() {
auto input_image = Image<float>::open (argument[0]).with_direct_io (3);
Eigen::MatrixXd grad = DWI::get_valid_DW_scheme (input_image);
// Want to support non-shell-like data if it's just a straight extraction
// of all dwis or all bzeros i.e. don't initialise the Shells class
std::vector<size_t> volumes;
bool bzero = get_options ("bzero").size();
auto opt = get_options ("shell");
if (opt.size()) {
DWI::Shells shells (grad);
shells.select_shells (false, false);
for (size_t s = 0; s != shells.count(); ++s) {
DEBUG ("Including data from shell b=" + str(shells[s].get_mean()) + " +- " + str(shells[s].get_stdev()));
for (std::vector<size_t>::const_iterator v = shells[s].get_volumes().begin(); v != shells[s].get_volumes().end(); ++v)
volumes.push_back (*v);
}
// Remove DW information from header if b=0 is the only 'shell' selected
bzero = (shells.count() == 1 && shells[0].is_bzero());
} else {
const float bzero_threshold = File::Config::get_float ("BValueThreshold", 10.0);
for (ssize_t row = 0; row != grad.rows(); ++row) {
if ((bzero && (grad (row, 3) < bzero_threshold)) || (!bzero && (grad (row, 3) > bzero_threshold)))
volumes.push_back (row);
}
}
if (volumes.empty()) {
auto type = (bzero) ? "b=0" : "dwi";
throw Exception ("No " + str(type) + " volumes present");
}
std::sort (volumes.begin(), volumes.end());
Header header (input_image);
if (volumes.size() == 1)
header.set_ndim (3);
else
header.size (3) = volumes.size();
Eigen::MatrixXd new_grad (volumes.size(), grad.cols());
for (size_t i = 0; i < volumes.size(); i++)
new_grad.row (i) = grad.row (volumes[i]);
header.set_DW_scheme (new_grad);
auto output_image = Image<float>::create (argument[1], header);
auto outer = Loop ("extracting volumes", input_image, 0, 3);
if (output_image.ndim() == 4) {
for (auto i = outer (output_image, input_image); i; ++i) {
for (size_t i = 0; i < volumes.size(); i++) {
input_image.index(3) = volumes[i];
output_image.index(3) = i;
output_image.value() = input_image.value();
}
}
} else {
const size_t volume = volumes[0];
for (auto i = outer (output_image, input_image); i; ++i) {
input_image.index(3) = volume;
output_image.value() = input_image.value();
}
}
}