Eigen::MatrixXf CombineMipmaps(const std::vector<Eigen::MatrixXf>& mm)
{
Eigen::MatrixXf r = Eigen::MatrixXf::Zero(Q*3*mm[0].rows()/2, Q*mm[0].cols());
r.block(0, 0, Q*mm[0].rows(), Q*mm[0].cols()) = density::ScaleUp(mm[0], Q);
unsigned int y = 0;
for(unsigned int i=1; i<mm.size(); ++i) {
r.block(Q*mm[0].rows(), y, Q*mm[i].rows(), Q*mm[i].cols()) = density::ScaleUp(mm[i], Q);
y += Q*mm[i].cols();
}
return r;
}
/** Randomly selects a point in the given tree node by considering probabilities
* Runtime: O(S*S + log(S*S))
*/
inline Eigen::Vector2f ProbabilityCellPoint(const Eigen::MatrixXf& m0, int scale, int x, int y)
{
x *= scale;
y *= scale;
const auto& b = m0.block(x, y, scale, scale);
// build cdf
std::vector<float> cdf(scale*scale);
for(int i=0; i<scale; ++i) {
for(int j=0; j<scale; ++j) {
float v = b(j,i);
int q = scale*i + j;
if(q > 0) {
cdf[q] = cdf[q-1] + v;
}
else {
cdf[q] = v;
}
}
}
// sample in cdf
boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > rnd(
Rnd(), boost::uniform_real<float>(0.0f, cdf.back()));
float v = rnd();
// find sample
auto it = std::lower_bound(cdf.begin(), cdf.end(), v);
int pos = std::distance(cdf.begin(), it);
return Eigen::Vector2f(x + pos%scale, y + pos/scale);
}
bool ZMeshAlgorithms::runBilateralFiltering(float sigma_r, float sigma_s)
{
int nSize = mesh_->get_num_of_faces_list();
int spatialDim = 3;
int rangeDim = 3;
Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormals(mesh_);
//Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormalsSpherical(mesh_);
Eigen::MatrixXf faceCenters = MeshTools::getAllFaceCenters(mesh_);
Eigen::MatrixXf verticePos = MeshTools::getAllVerticePos(mesh_);
Eigen::MatrixXf faceAttributes(nSize, spatialDim+rangeDim);
faceAttributes.block(0, 0, nSize, spatialDim) = faceCenters;
faceAttributes.block(0, spatialDim, nSize, rangeDim) = faceNormals;
AnnWarper_Eigen annSearch;
annSearch.init(faceCenters);
ZMeshBilateralFilter filter(mesh_);
filter.setAnnSearchHandle(&annSearch);
float sigma_spatial = mesh_->m_minEdgeLength*sigma_s;
float sigma_range = cos(sigma_r*Z_PI/180.f);
//filter.setKernelFunc(NULL);
filter.setPara(ZMeshFilterParaNames::SpatialSigma, sigma_spatial);
filter.setPara(ZMeshFilterParaNames::RangeSigma, sigma_range);
filter.apply(faceAttributes, std::vector<bool>(nSize, true));
Eigen::MatrixXf output = filter.getResult();
MeshTools::setAllFaceNormals2(mesh_, output.block(0, 3, nSize, 3));
//MeshTools::setAllFaceNormal2Spherical(mesh_, output.block(0, spatialDim, nSize, rangeDim));
return true;
}
void addEigenMatrixRow( Eigen::MatrixXf &m )
{
Eigen::MatrixXf temp=m;
m.resize(m.rows()+1,m.cols());
m.setZero();
m.block(0,0,temp.rows(),temp.cols())=temp;
}
void ZMeshAlgorithms::updateRange()
{
pMeshFilterManifold_->updateRange();
Eigen::MatrixXf output = pMeshFilterManifold_->getResult();
meshNewNormals_ = output.block(0, pMeshFilterManifold_->getSpatialDim(), pMeshFilterManifold_->getPointSize(), pMeshFilterManifold_->getRangeDim());
MeshTools::setAllFaceNormals2(mesh_, meshNewNormals_);
//MeshTools::setAllFaceNormal2Spherical(mesh_, meshNewNormals_);
}
bool ZMeshAlgorithms::runManifoldSmooth(float sigma_r, float sigma_s)
{
SAFE_DELETE(pMeshFilterManifold_);
int nSize = mesh_->get_num_of_faces_list();
//int nSize = mesh_->get_num_of_vertex_list();
int spatialDim = 3;
int rangeDim = 3;
Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormals(mesh_);
//Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormalsSpherical(mesh_);
Eigen::MatrixXf faceCenters = MeshTools::getAllFaceCenters(mesh_);
Eigen::MatrixXf verticePos = MeshTools::getAllVerticePos(mesh_);
Eigen::MatrixXf faceAttributes(nSize, spatialDim+rangeDim);
faceAttributes.block(0, 0, nSize, spatialDim) = faceCenters;
faceAttributes.block(0, 3, nSize, rangeDim) = faceNormals;
// faceAttributes.block(0, 0, nSize, spatialDim) = verticePos;
// faceAttributes.block(0, 3, nSize, rangeDim) = verticePos;
//ZMeshFilterManifold filter(mesh_);
pMeshFilterManifold_ = new ZMeshFilterManifold(mesh_);
float sigma_spatial = mesh_->m_minEdgeLength*sigma_s;
float sigma_range = sin(sigma_r*Z_PI/180.f);
pMeshFilterManifold_->setPara(ZMeshFilterParaNames::SpatialSigma, sigma_spatial);
pMeshFilterManifold_->setPara(ZMeshFilterParaNames::RangeSigma, sigma_range);
pMeshFilterManifold_->setRangeDim(rangeDim);
pMeshFilterManifold_->setSpatialDim(spatialDim);
CHECK_FALSE_AND_RETURN(pMeshFilterManifold_->apply(faceAttributes, std::vector<bool>(nSize, true)));
Eigen::MatrixXf output = pMeshFilterManifold_->getResult();
meshNewNormals_ = output.block(0, spatialDim, nSize, rangeDim);
MeshTools::setAllFaceNormals2(mesh_, meshNewNormals_);
MeshTools::setAllFaceColorValue(mesh_, pMeshFilterManifold_->getPointClusterIds());
//MeshTools::setAllVerticePos(mesh_, output.block(0, spatialDim, nSize, rangeDim));
//MeshTools::setAllFaceNormal2Spherical(mesh_, output.block(0, spatialDim, nSize, rangeDim));
ZFileHelper::saveEigenMatrixToFile("oldNormal.txt", faceNormals);
ZFileHelper::saveEigenMatrixToFile("newNormal.txt", output.block(0,spatialDim, nSize, rangeDim));
return true;
}
void ZMeshFilterManifold::init(const Eigen::MatrixXf& input)
{
destroy();
computeTreeHeight();
initMinPixelDist2Manifold();
// init search tool
pAnnSearch_ = new AnnWarper_Eigen;
//Eigen::MatrixXf faceCenters = MeshTools::getAllFaceCenters(pMesh_);
//Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormals(pMesh_);
pAnnSearch_->init(input.block(0, 0, input.rows(), spatialDim_));
// init gaussian filter
pGaussianFilter_ = new ZMeshBilateralFilter(pMesh_);
ZMeshBilateralFilter* pFilter = (ZMeshBilateralFilter*)pGaussianFilter_;
pFilter->setAnnSearchHandle(pAnnSearch_);
}
template <typename PointT> void
pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const Eigen::Vector4f ¢roid,
Eigen::MatrixXf &cloud_out)
{
size_t npts = cloud_in.points.size ();
cloud_out = Eigen::MatrixXf::Zero (4, npts); // keep the data aligned
for (size_t i = 0; i < npts; ++i)
// One column at a time
cloud_out.block<4, 1> (0, i) = cloud_in.points[i].getVector4fMap () - centroid;
// Make sure we zero the 4th dimension out (1 row, N columns)
cloud_out.block (3, 0, 1, npts).setZero ();
}
IGL_INLINE void igl::fit_rotations_AVX(
const Eigen::MatrixXf & S,
Eigen::MatrixXf & R)
{
const int cStep = 8;
assert(S.cols() == 3);
const int dim = 3; //S.cols();
const int nr = S.rows()/dim;
assert(nr * dim == S.rows());
// resize output
R.resize(dim,dim*nr); // hopefully no op (should be already allocated)
Eigen::Matrix<float, 3*cStep, 3> siBig;
// using SSE decompose cStep matrices at a time:
int r = 0;
for( ; r<nr; r+=cStep)
{
int numMats = cStep;
if (r + cStep >= nr) numMats = nr - r;
// build siBig:
for (int k=0; k<numMats; k++)
{
for(int i = 0;i<dim;i++)
{
for(int j = 0;j<dim;j++)
{
siBig(i + 3*k, j) = S(i*nr + r + k, j);
}
}
}
Eigen::Matrix<float, 3*cStep, 3> ri;
polar_svd3x3_avx(siBig, ri);
for (int k=0; k<cStep; k++)
assert(ri.block(3*k, 0, 3, 3).determinant() >= 0);
// Not sure why polar_dec computes transpose...
for (int k=0; k<numMats; k++)
{
R.block(0, (r + k)*dim, dim, dim) = ri.block(3*k, 0, dim, dim).transpose();
}
}
}
/**
* DemeanPoints
*/
void RigidTransformationRANSAC::DemeanPoints (
const std::vector<Eigen::Vector3f > &inPts,
const std::vector<int> &indices,
const Eigen::Vector3f ¢roid,
Eigen::MatrixXf &outPts)
{
if (inPts.size()==0 || indices.size()==0)
{
return;
}
outPts = Eigen::MatrixXf(4, indices.size());
// Subtract the centroid from points
for (unsigned i = 0; i < indices.size(); i++)
outPts.block<3,1>(0,i) = inPts[indices[i]]-centroid;
outPts.block(3, 0, 1, indices.size()).setZero();
}
/** Selects the point in the tree node with highest probability */
inline Eigen::Vector2f OptimalCellPoint(const Eigen::MatrixXf& m0, int scale, int x, int y)
{
x *= scale;
y *= scale;
const auto& b = m0.block(x, y, scale, scale);
unsigned int best_j=scale/2, best_i=scale/2;
float best_val = -1000000.0f;
for(unsigned int i=0; i<b.cols(); ++i) {
for(unsigned int j=0; j<b.rows(); ++j) {
float val = b(j,i);
if(val > best_val) {
best_j = j;
best_i = i;
best_val = val;
}
}
}
return Eigen::Vector2f(x + best_j, y + best_i);
}
/** Randomly selects a point in the given tree node by considering probabilities
* Assumes that cdf_sum is the probability sum in the given tree node
* Runtime: O(S*S/2)
*/
inline Eigen::Vector2f ProbabilityCellPoint(const Eigen::MatrixXf& m0, int scale, int x, int y, float cdf_sum)
{
// sample in cdf
boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > rnd(
Rnd(), boost::uniform_real<float>(0.0f, cdf_sum));
float v = rnd();
// find sample
x *= scale;
y *= scale;
const auto& b = m0.block(x, y, scale, scale);
for(int i=0; i<scale; ++i) {
for(int j=0; j<scale; ++j) {
v -= b(j,i);
if(v <= 0.0f) {
return Eigen::Vector2f(x + j, y + i);
}
}
}
// should never be here
assert(false);
return Eigen::Vector2f(x + scale/2, y + scale/2);
}
void compressFeature( string filename, std::vector< std::vector<float> > &models, const int dim, bool ascii ){
PCA pca;
pca.read( filename.c_str(), ascii );
Eigen::VectorXf variance = pca.getVariance();
Eigen::MatrixXf tmpMat = pca.getAxis();
Eigen::MatrixXf tmpMat2 = tmpMat.block(0,0,tmpMat.rows(),dim);
const int num = (int)models.size();
for( int i=0; i<num; i++ ){
Eigen::Map<Eigen::VectorXf> vec( &(models[i][0]), models[i].size() );
//vec = tmpMat2.transpose() * vec;
Eigen::VectorXf tmpvec = tmpMat2.transpose() * vec;
models[i].resize( dim );
if( WHITENING ){
for( int t=0; t<dim; t++ )
models[i][t] = tmpvec[t] / sqrt( variance( t ) );
}
else{
for( int t=0; t<dim; t++ )
models[i][t] = tmpvec[t];
}
}
}
Eigen::MatrixXf LQRControler::Controler(Eigen::MatrixXf states,Eigen::MatrixXf ref,bool stop){
if(stop){
xs.setZero();
xr.setZero();
deltaxsi.setZero();
xsi.setZero();
xsiant.setZero();
deltaxsi.setZero();
deltaxsiant.setZero();
deltaxs.setZero();
xs_aumented.setZero();
deltaxs.setZero();
xsi.setZero();
deltaU.setZero();
xs_aumented.setZero();
ur.setZero();
auxu.setZero();
ur.setZero();
deltaU.setZero();
xsiant.setZero();
xsi.setZero();
deltaxsiant.setZero();
deltaxsi.setZero();
}else{
//Vectors of reference trajectory and control
xs<<0,0,states(2),states.block(3,0,5,1),0,0,states(10),states.block(11,0,5,1);
xr=trajectory->TrajetoryReference_LQR(ref);
//Vector integration of error(Trapezoidal method)
deltaxsi<<xs(2,0)-xr(2,0),xs(5,0)-xr(5,0);
xsi=xsiant+ts*(deltaxsi+deltaxsiant)/2;
// Error state vector
deltaxs=xs-xr;
// augmented error state vector
xs_aumented<<deltaxs,xsi;
//Control action variation
deltaU=Ke*xs_aumented;
//Control reference
ur<<9857.54,9837.48,0,0;
// Total control action
auxu=ur+deltaU;
//Variable update
xsiant=xsi;
deltaxsiant=deltaxsi;
}
if(auxu(0,0)>15000 ){
auxu(0,0)=15000;
}
if(auxu(1,0)>15000 ){
auxu(1,0)=15000;
}
/*The mass in the mathematical model was taken in grams,
for this reason the controller calculate the forces in g .m/s^2 and the torque in g .m^2/s^2.
But, the actuators are in the international units N and N. m for this reason the controls
actions are transforming from g to Kg*/
u(0,0)=auxu(0,0)/1000;
u(1,0)=auxu(1,0)/1000;
u(2,0)=auxu(2,0)/1000;
u(3,0)=auxu(3,0)/1000;
return u;
}
template<typename PointT> void
pcl::registration::LUM<PointT>::compute ()
{
int n = static_cast<int> (getNumVertices ());
if (n < 2)
{
PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n");
return;
}
for (int i = 0; i < max_iterations_; ++i)
{
// Linearized computation of C^-1 and C^-1*D and convergence checking for all edges in the graph (results stored in slam_graph_)
typename SLAMGraph::edge_iterator e, e_end;
for (boost::tuples::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e)
computeEdge (*e);
// Declare matrices G and B
Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1));
Eigen::VectorXf B = Eigen::VectorXf::Zero (6 * (n - 1));
// Start at 1 because 0 is the reference pose
for (int vi = 1; vi != n; ++vi)
{
for (int vj = 0; vj != n; ++vj)
{
// Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge
Edge e;
bool present1, present2;
boost::tuples::tie (e, present1) = edge (vi, vj, *slam_graph_);
if (!present1)
{
boost::tuples::tie (e, present2) = edge (vj, vi, *slam_graph_);
if (!present2)
continue;
}
// Fill in elements of G and B
if (vj > 0)
G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
}
}
// Computation of the linear equation system: GX = B
// TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse)
Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B);
// Update the poses
float sum = 0.0;
for (int vi = 1; vi != n; ++vi)
{
Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f> (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6));
sum += difference_pose.norm ();
setPose (vi, getPose (vi) + difference_pose);
}
// Convergence check
if (sum <= convergence_threshold_ * static_cast<float> (n - 1))
return;
}
}
//********************************
//* main
int main(int argc, char* argv[]) {
if( (argc != 13) && (argc != 15) ){
std::cerr << "usage: " << argv[0] << " [path] <rank_num> <exist_voxel_num_threshold> [model_pca_filename] <dim_model> <size1> <size2> <size3> <detect_th> <distance_th> <model_num> /input:=/camera/rgb/points" << std::endl;
exit( EXIT_FAILURE );
}
char tmpname[ 1000 ];
ros::init (argc, argv, "detect_object_vosch_multi", ros::init_options::AnonymousName);
// read the length of voxel side
sprintf( tmpname, "%s/param/parameters.txt", argv[1] );
voxel_size = Param::readVoxelSize( tmpname );
detect_th = atof( argv[9] );
distance_th = atof( argv[10] );
model_num = atoi( argv[11] );
rank_num = atoi( argv[2] );
// set marker color
const int marker_model_num = 6;
if( model_num > marker_model_num ){
std::cerr << "Please set more marker colors for detection of more than " << marker_model_num << " objects." << std::endl;
exit( EXIT_FAILURE );
}
marker_color_r = new float[ marker_model_num ];
marker_color_g = new float[ marker_model_num ];
marker_color_b = new float[ marker_model_num ];
marker_color_r[ 0 ] = 1.0; marker_color_g[ 0 ] = 0.0; marker_color_b[ 0 ] = 0.0; // red
marker_color_r[ 1 ] = 0.0; marker_color_g[ 1 ] = 1.0; marker_color_b[ 1 ] = 0.0; // green
marker_color_r[ 2 ] = 0.0; marker_color_g[ 2 ] = 0.0; marker_color_b[ 2 ] = 1.0; // blue
marker_color_r[ 3 ] = 1.0; marker_color_g[ 3 ] = 1.0; marker_color_b[ 3 ] = 0.0; // yellow
marker_color_r[ 4 ] = 0.0; marker_color_g[ 4 ] = 1.0; marker_color_b[ 4 ] = 1.0; // cyan
marker_color_r[ 5 ] = 1.0; marker_color_g[ 5 ] = 0.0; marker_color_b[ 5 ] = 1.0; // magenta
// marker_color_r[ 0 ] = 0.0; marker_color_g[ 0 ] = 1.0; marker_color_b[ 0 ] = 0.0; // green
// marker_color_r[ 1 ] = 0.0; marker_color_g[ 1 ] = 0.0; marker_color_b[ 1 ] = 1.0; // blue
// marker_color_r[ 2 ] = 0.0; marker_color_g[ 2 ] = 1.0; marker_color_b[ 2 ] = 1.0; // cyan
// marker_color_r[ 3 ] = 1.0; marker_color_g[ 3 ] = 0.0; marker_color_b[ 3 ] = 0.0; // pink
// read the number of voxels in each subdivision's side of scene
box_size = Param::readBoxSizeScene( tmpname );
// read the dimension of compressed feature vectors
dim = Param::readDim( tmpname );
const int dim_model = atoi(argv[5]);
if( dim <= dim_model ){
std::cerr << "ERR: dim_model should be less than dim(in dim.txt)" << std::endl;
exit( EXIT_FAILURE );
}
// read the threshold for RGB binalize
sprintf( tmpname, "%s/param/color_threshold.txt", argv[1] );
Param::readColorThreshold( color_threshold_r, color_threshold_g, color_threshold_b, tmpname );
// determine the size of sliding box
region_size = box_size * voxel_size;
float tmp_val = atof(argv[6]) / region_size;
int size1 = (int)tmp_val;
if( ( ( tmp_val - size1 ) >= 0.5 ) || ( size1 == 0 ) ) size1++;
tmp_val = atof(argv[7]) / region_size;
int size2 = (int)tmp_val;
if( ( ( tmp_val - size2 ) >= 0.5 ) || ( size2 == 0 ) ) size2++;
tmp_val = atof(argv[8]) / region_size;
int size3 = (int)tmp_val;
if( ( ( tmp_val - size3 ) >= 0.5 ) || ( size3 == 0 ) ) size3++;
sliding_box_size_x = size1 * region_size;
sliding_box_size_y = size2 * region_size;
sliding_box_size_z = size3 * region_size;
// set variables
search_obj.setModelNum( model_num );
#ifdef CCHLAC_TEST
sprintf( tmpname, "%s/param/max_c.txt", argv[1] );
#else
sprintf( tmpname, "%s/param/max_r.txt", argv[1] );
#endif
search_obj.setNormalizeVal( tmpname );
search_obj.setRange( size1, size2, size3 );
search_obj.setRank( rank_num * model_num ); // for removeOverlap()
search_obj.setThreshold( atoi(argv[3]) );
// read projection axes of the target objects' subspace
FILE *fp = fopen( argv[4], "r" );
char **model_file_names = new char* [ model_num ];
char line[ 1000 ];
for( int i=0; i<model_num; i++ ){
model_file_names[ i ] = new char [ 1000 ];
if( fgets( line, sizeof(line), fp ) == NULL ) std::cerr<<"fgets err"<<std::endl;
line[ strlen( line ) - 1 ] = '\0';
//fscanf( fp, "%s\n", model_file_names + i );
//model_file_names[ i ] = line;
sprintf( model_file_names[ i ], "%s", line );
//std::cout << model_file_names[ i ] << std::endl;
}
fclose(fp);
search_obj.readAxis( model_file_names, dim, dim_model, ASCII_MODE_P, MULTIPLE_SIMILARITY );
// read projection axis for feature compression
PCA pca;
sprintf( tmpname, "%s/models/compress_axis", argv[1] );
pca.read( tmpname, ASCII_MODE_P );
Eigen::MatrixXf tmpaxis = pca.getAxis();
Eigen::MatrixXf axis = tmpaxis.block( 0,0,tmpaxis.rows(),dim );
Eigen::MatrixXf axis_t = axis.transpose();
Eigen::VectorXf variance = pca.getVariance();
if( WHITENING )
search_obj.setSceneAxis( axis_t, variance, dim );
else
search_obj.setSceneAxis( axis_t );
// object detection
VoxelizeAndDetect vad;
vad.loop();
ros::spin();
return 0;
}
template<typename PointT> inline void
pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
{
if (!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
const size_t n = eigenvectors_.cols ();// number of eigen vectors
Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
Eigen::VectorXf h = y - input;
if (h.norm() > 0)
h.normalize ();
else
h.setZero ();
float gamma = h.dot(input - mean_.head<3>());
Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
D.block(0,0,n,n) = a * a.transpose();
D /= float(n)/float((n+1) * (n+1));
for(std::size_t i=0; i < a.size(); i++) {
D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
D(i,D.cols()-1) = D(D.rows()-1,i);
D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
}
Eigen::MatrixXf R(D.rows(), D.cols());
Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
Eigen::VectorXf alphap = D_evd.eigenvalues().real();
eigenvalues_.resize(eigenvalues_.size() +1);
for(std::size_t i=0; i<eigenvalues_.size(); i++) {
eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
R.col(i) = D.col(D.cols()-i-1);
}
Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
Up.rightCols<1>() = h;
eigenvectors_ = Up*R;
if (!basis_only_) {
Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
coefficients_(coefficients_.rows()-1,i) = 0;
coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
}
a.resize(a.size()+1);
a(a.size()-1) = 0;
coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
}
mean_.head<3>() = meanp;
switch (flag)
{
case increase:
if (eigenvectors_.rows() >= eigenvectors_.cols())
break;
case preserve:
if (!basis_only_)
coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
eigenvalues_.resize(eigenvalues_.size()-1);
break;
default:
PCL_ERROR("[pcl::PCA] unknown flag\n");
}
}
//********************************
//* main
int main(int argc, char* argv[]) {
if( (argc != 12) && (argc != 14) ){
std::cerr << "usage: " << argv[0] << " [path] <rank_num> <exist_voxel_num_threshold> [model_pca_filename] <dim_model> <size1> <size2> <size3> <detect_th> <distance_th> /input:=/camera/rgb/points" << std::endl;
exit( EXIT_FAILURE );
}
char tmpname[ 1000 ];
ros::init (argc, argv, "detectObj", ros::init_options::AnonymousName);
// read the length of voxel side
sprintf( tmpname, "%s/param/parameters.txt", argv[1] );
voxel_size = Param::readVoxelSize( tmpname );
detect_th = atof( argv[9] );
distance_th = atof( argv[10] );
rank_num = atoi( argv[2] );
// read the number of voxels in each subdivision's side of scene
box_size = Param::readBoxSizeScene( tmpname );
// read the dimension of compressed feature vectors
dim = Param::readDim( tmpname );
// set the dimension of the target object's subspace
const int dim_model = atoi(argv[5]);
if( dim <= dim_model ){
std::cerr << "ERR: dim_model should be less than dim(in dim.txt)" << std::endl;
exit( EXIT_FAILURE );
}
// read the threshold for RGB binalize
sprintf( tmpname, "%s/param/color_threshold.txt", argv[1] );
Param::readColorThreshold( color_threshold_r, color_threshold_g, color_threshold_b, tmpname );
// determine the size of sliding box
region_size = box_size * voxel_size;
float tmp_val = atof(argv[6]) / region_size;
int size1 = (int)tmp_val;
if( ( ( tmp_val - size1 ) >= 0.5 ) || ( size1 == 0 ) ) size1++;
tmp_val = atof(argv[7]) / region_size;
int size2 = (int)tmp_val;
if( ( ( tmp_val - size2 ) >= 0.5 ) || ( size2 == 0 ) ) size2++;
tmp_val = atof(argv[8]) / region_size;
int size3 = (int)tmp_val;
if( ( ( tmp_val - size3 ) >= 0.5 ) || ( size3 == 0 ) ) size3++;
sliding_box_size_x = size1 * region_size;
sliding_box_size_y = size2 * region_size;
sliding_box_size_z = size3 * region_size;
// set variables
search_obj.setRange( size1, size2, size3 );
search_obj.setRank( rank_num );
search_obj.setThreshold( atoi(argv[3]) );
search_obj.readAxis( argv[4], dim, dim_model, ASCII_MODE_P, MULTIPLE_SIMILARITY );
// read projection axis of the target object's subspace
PCA pca;
sprintf( tmpname, "%s/models/compress_axis", argv[1] );
pca.read( tmpname, ASCII_MODE_P );
Eigen::MatrixXf tmpaxis = pca.getAxis();
Eigen::MatrixXf axis = tmpaxis.block( 0,0,tmpaxis.rows(),dim );
Eigen::MatrixXf axis_t = axis.transpose();
Eigen::VectorXf variance = pca.getVariance();
if( WHITENING )
search_obj.setSceneAxis( axis_t, variance, dim );
else
search_obj.setSceneAxis( axis_t );
// object detection
VoxelizeAndDetect vad;
vad.loop();
ros::spin();
return 0;
}
void linearTMatrixTest(StrainLin * ene)
{
ElementRegGrid * grid = new ElementRegGrid(1, 1, 1);
MaterialQuad * material = new MaterialQuad(ene);
grid->m.push_back(material);
grid->x[1][0] += 0.1f;
grid->x[3][1] += 0.2f;
MatrixXf K = grid->getStiffness();
//linear material stiffness
ElementHex * ele = (ElementHex*)grid->e[0];
const Quadrature & q = Quadrature::Gauss2;
Eigen::MatrixXf Ka = Eigen::MatrixXf::Zero(3 * ele->nV(), 3 * ele->nV());
Eigen::MatrixXf E = ene->EMatrix();
Eigen::VectorXf U = Eigen::VectorXf::Zero(3 * ele->nV());
for (int ii = 0; ii < ele->nV(); ii++){
for (int jj = 0; jj < 3; jj++){
U[3 * ii + jj] = grid->x[ii][jj] - grid->X[ii][jj];
}
}
for (unsigned int ii = 0; ii < q.x.size(); ii++){
Eigen::MatrixXf B = ele->BMatrix(q.x[ii], grid->X);
Eigen::MatrixXf ss = E*B*U;
//std::cout <<"sigma:\n"<< ss << "\n";
Matrix3f F = ele->defGrad(q.x[ii], grid->X, grid->x);
Matrix3f P = ene->getPK1(F);
//std::cout << "P:\n";
//P.print();
Ka += q.w[ii] * B.transpose() * E * B;
//std::cout << "B:\n" << B << "\n";
}
//std::cout << "E:\n" << E << "\n";
//std::cout << "alt K:\n";
//std::cout << Ka << "\n";
float maxErr = 0;
for (int ii = 0; ii<K.mm; ii++){
for (int jj = 0; jj<K.nn; jj++){
float err = (float)std::abs(Ka(ii, jj) - K(ii, jj));
if (err>maxErr){
maxErr = err;
}
}
}
std::cout << "max err " << maxErr << "\n";
//assemble boundary matrix HNEB
std::ofstream out("T.txt");
// 2 point quadrature is accurate enough
const Quadrature & q2d = Quadrature::Gauss2_2D;
Eigen::MatrixXf T = Eigen::MatrixXf::Zero(3 * ele->nV(), 3 * ele->nV());
for (int ii = 0; ii < ele->nF(); ii++){
Eigen::MatrixXf Tf = Eigen::MatrixXf::Zero(3 * ele->nV(), 3 * ele->nV());
Eigen::MatrixXf N = ele->NMatrix(ii);
N.block(0, 3, 3, 3) = Eigen::MatrixXf::Zero(3, 3);
//std::cout << "N:\n"<<N << "\n";
for (unsigned int jj = 0; jj < q2d.x.size(); jj++){
Vector3f quadp = ele->facePt(ii, q2d.x[jj]);
Eigen::MatrixXf B0 = ele->BMatrix(quadp, grid->X);
Eigen::MatrixXf B = Eigen::MatrixXf::Zero(6, 3 * ele->nV());
//only add contributions from the face
for (int kk = 0; kk < 4; kk++){
int vidx = faces[ii][kk];
B.block(0, 3 * vidx, 6, 3) = B0.block(0, 3 * vidx, 6, 3);
}
//B=B0;
Eigen::MatrixXf H = ele->HMatrix(quadp);
//std::cout << "H:\n" << H.transpose() << "\n";
//std::cout << "B:\n" << B.transpose() << "\n";
//std::cout << "traction:\n";
//Tf += q2d.w[jj] * H.transpose() * N * E * B;
Tf += q2d.w[jj] * H.transpose() * N * E * N.transpose() * H;
//Tf += q2d.w[jj] * B.transpose() * E * B;
Eigen::Vector3f surfF = (N * E * B * U);
//std::cout << surfF << "\n";
Matrix3f F = ele->defGrad(quadp, grid->X, grid->x);
Matrix3f P = ene->getPK1(F);
Vector3f surfF1 = P * Vector3f(facen[ii][0], facen[ii][1], facen[ii][2]);
std::cout << surfF1[0] << " " << surfF1[1] << " " << surfF1[2] << "\n";
}
//out << Tf << "\n\n";
T += Tf;
}
//out << T << "\n\n";
//out << Ka << "\n";
out.close();
}
bool ZMeshFilterManifold::buildManifoldAndPerformFiltering(const Eigen::MatrixXf& input,
const Eigen::MatrixXf& etaK, const std::vector<bool>& clusterK,
float sigma_s, float sigma_r, int currentTreeLevel)
{
int inputSize = input.rows();;
static std::string fileName("etaK.txt");
fileName += "0";
ZFileHelper::saveEigenMatrixToFile(fileName.c_str(), etaK);
// splatting: project the vertices onto the current manifold etaK
// NOTE: the sigma should be recursive!!
float sigmaR_over_sqrt_2 = sigma_r/sqrt(2.0);
Eigen::MatrixXf diffX = input.block(0, spatialDim_, inputSize, rangeDim_) - etaK.block(0, spatialDim_, inputSize, rangeDim_);
Eigen::VectorXf gaussianDistWeight(inputSize);
//std::cout << "diffX=\n" << diffX.block(0,0,10,rangeDim_+spatialDim_) << "\n";
for (int i=0; i<inputSize; i++)
{
gaussianDistWeight(i) = ZKernelFuncs::GaussianKernelFunc(diffX.row(i), sigmaR_over_sqrt_2);
}
Eigen::MatrixXf psiSplat(inputSize, spatialDim_+rangeDim_+1);
Eigen::VectorXf psiSplat0 = gaussianDistWeight;
//////////////////////////////////////////////////////////////////////////
// for debug
//std::stringstream ss;
static int etaI = 1;
//ss << "gaussianWeights" << etaI << ".txt";
//std::cout << ZConsoleTools::green << etaI << "\n" << ZConsoleTools::white;
etaI++;
//ZFileHelper::saveEigenVectorToFile(ss.str().c_str(), gaussianDistWeight);
//////////////////////////////////////////////////////////////////////////
psiSplat.block(0,0,inputSize,spatialDim_) = input.block(0,0,inputSize,spatialDim_);
for (int i=0; i<inputSize; i++)
{
//psiSplat.block(i, spatialDim_, 1, rangeDim_) = input.block(i, spatialDim_, 1, rangeDim_)*gaussianDistWeight(i);
psiSplat.block(i, spatialDim_, 1, rangeDim_) = etaK.block(i, spatialDim_, 1, rangeDim_)*gaussianDistWeight(i);
}
psiSplat.col(spatialDim_+rangeDim_) = gaussianDistWeight;
// save min distance to later perform adjustment of outliers -- Eq.(10)
// UNDO
// update min_pixel_dist_to_manifold_square_
Eigen::VectorXf pixelDist2Manifold(inputSize);
for (int i=0; i<inputSize; i++)
{
if (clusterK[i])
pixelDist2Manifold(i) = diffX.row(i).squaredNorm();
else
pixelDist2Manifold(i) = FLT_MAX;
}
min_pixel_dist_to_manifold_squared_ = min_pixel_dist_to_manifold_squared_.cwiseMin(pixelDist2Manifold);
// Blurring
Eigen::MatrixXf wkiPsiBlur(inputSize, rangeDim_);
Eigen::VectorXf wkiPsiBlur0(inputSize);
//Eigen::MatrixXf psiSplatAnd0(inputSize, spatialDim_+rangeDim_+1);
//psiSplatAnd0.block(0, 0, inputSize, spatialDim_+rangeDim_) = psiSplat.block(0, 0, inputSize, spatialDim_+rangeDim_);
//psiSplatAnd0.col(spatialDim_+rangeDim_) = psiSplat0;
//psiSplatAnd0.block(0, 0, inputSize, spatialDim_+rangeDim_) = input;
//psiSplatAnd0.col(spatialDim_+rangeDim_).fill(1.f);
//pGaussianFilter_->setKernelFunc(ZKernelFuncs::GaussianKernelFuncNormal3);
pGaussianFilter_->setKernelFunc(ZKernelFuncs::GaussianKernelFuncA);
//pGaussianFilter_->setKernelFunc(NULL);
//pGaussianFilter_->setKernelFunc(ZKernelFuncs::GaussianKernelSphericalFunc);
pGaussianFilter_->setPara(ZMeshFilterParaNames::SpatialSigma, sigma_s);
pGaussianFilter_->setPara(ZMeshFilterParaNames::RangeSigma, sigmaR_over_sqrt_2);
//pGaussianFilter_->apply(psiSplatAnd0, clusterK);
//CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(psiSplatAnd0, spatialDim_, rangeDim_+1, Eigen::VectorXf(), clusterK));
CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(psiSplat, spatialDim_, rangeDim_+1, gaussianDistWeight, clusterK));
Eigen::MatrixXf wkiPsiBlurAnd0 = pGaussianFilter_->getResult();
wkiPsiBlur = wkiPsiBlurAnd0.block(0, spatialDim_, inputSize, rangeDim_);
wkiPsiBlur0 = wkiPsiBlurAnd0.col(spatialDim_+rangeDim_);
//ZFileHelper::saveEigenMatrixToFile("")
// std::cout << "wkiPsiBlurAnd0=\n" << wkiPsiBlurAnd0.block(0,0,10,rangeDim_+spatialDim_+1) << "\n";
// std::cout << "wkiPsiBlur0=\n" << wkiPsiBlur0.head(10) << "\n";
//std::cout << "pixelDist2Manifold=\n" << pixelDist2Manifold.head(10) << "\n";
//std::cout << "min_pixel_dist_to_manifold_squared=\n" << min_pixel_dist_to_manifold_squared_.head(10) << "\n";
// for debug
// for (int i=0; i<inputSize; i++)
// {
// if (!g_isInfinite(wkiPsiBlur(i,0)))
// std::cout << "(" << i << "," << 0 << ")\n";
// if (!g_isInfinite(wkiPsiBlur(i,1)))
// std::cout << "(" << i << "," << 1 << ")\n";
// //if (!g_isInfinite(wkiPsiBlur(i,2)))
// // std::cout << "(" << i << "," << 2 << ")\n";
// }
//std::cout << wkiPsiBlur.norm() << "\n";
Eigen::VectorXf rangeDiff(inputSize);
for (int i=0; i<inputSize; i++)
{
Eigen::VectorXf n0 = wkiPsiBlur.row(i);
Eigen::VectorXf n1 = input.row(i).tail(rangeDim_);
n0.normalize();
n1.normalize();
rangeDiff(i) = 1.f-n0.dot(n1);
}
static bool bSaved = false;
if (!bSaved)
{
ZFileHelper::saveEigenVectorToFile("rangeDiff.txt", rangeDiff);
ZFileHelper::saveEigenVectorToFile("gaussian.txt", gaussianDistWeight);
ZFileHelper::saveEigenMatrixToFile("splat.txt", psiSplat);
ZFileHelper::saveEigenMatrixToFile("blur.txt", wkiPsiBlur);
bSaved = true;
}
// Slicing
Eigen::VectorXf wki = gaussianDistWeight;
for (int i=0; i<inputSize; i++)
{
if (!clusterK[i]) continue;
sum_w_ki_Psi_blur_.row(i) += wkiPsiBlur.row(i)*wki(i);
sum_w_ki_Psi_blur_0_(i) += wkiPsiBlur0(i)*wki(i);
}
//////////////////////////////////////////////////////////////////////////
// for debug
wki_Psi_blurs_.push_back(Eigen::MatrixXf(inputSize, rangeDim_));
wki_Psi_blur_0s_.push_back(Eigen::VectorXf(inputSize));
Eigen::MatrixXf& lastM = wki_Psi_blurs_[wki_Psi_blurs_.size()-1];
lastM.fill(0);
Eigen::VectorXf& lastV = wki_Psi_blur_0s_[wki_Psi_blur_0s_.size()-1];
lastV.fill(0);
for (int i=0; i<inputSize; i++)
{
if (!clusterK[i]) continue;
lastM.row(i) = wkiPsiBlur.row(i)*wki(i);
lastV(i) = wkiPsiBlur0(i)*wki(i);
}
std::cout << sum_w_ki_Psi_blur_.norm() << "\n";
//////////////////////////////////////////////////////////////////////////
// compute two new manifolds eta_minus and eta_plus
// test stopping criterion
if (currentTreeLevel<filterPara_.tree_height)
{
// algorithm 1, Step 2: compute the eigenvector v1
Eigen::VectorXf v1 = computeMaxEigenVector(diffX, clusterK);
// algorithm 1, Step 3: Segment vertices into two clusters
std::vector<bool> clusterMinus(inputSize, false);
std::vector<bool> clusterPlus(inputSize, false);
int countMinus=0;
int countPlus =0;
for (int i=0; i<inputSize; i++)
{
float dot = diffX.row(i).dot(v1);
if (dot<0 && clusterK[i]) {countMinus++; verticeClusterIds[i] =etaI+0.5; clusterMinus[i] = true;}
if (dot>=0 && clusterK[i]) {countPlus++; verticeClusterIds[i] =etaI-0.5; clusterPlus[i] = true;}
}
std::cout << "Minus manifold: " << countMinus << "\n";
std::cout << "Plus manifold: " << countPlus << "\n";
// Eigen::MatrixXf diffXManifold(inputSize, spatialDim_+rangeDim_);
// diffXManifold.block(0, 0, inputSize, spatialDim_) = input.block(0, 0, inputSize, spatialDim_);
// diffXManifold.block(0, spatialDim_, inputSize, rangeDim_) = diffX;
// algorithm 1, Step 4: Compute new manifolds by weighted low-pass filtering -- Eq. (7)(8)
Eigen::VectorXf theta(inputSize);
theta.fill(1);
theta = theta - wki.cwiseProduct(wki);
pGaussianFilter_->setKernelFunc(NULL);
CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(input, spatialDim_, rangeDim_, theta, clusterMinus));
Eigen::MatrixXf etaMinus = pGaussianFilter_->getResult();
CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(input, spatialDim_, rangeDim_, theta, clusterPlus));
Eigen::MatrixXf etaPlus = pGaussianFilter_->getResult();
// algorithm 1, Step 5: recursively build more manifolds
CHECK_FALSE_AND_RETURN(buildManifoldAndPerformFiltering(input, etaMinus, clusterMinus, sigma_s, sigma_r, currentTreeLevel+1));
CHECK_FALSE_AND_RETURN(buildManifoldAndPerformFiltering(input, etaPlus, clusterPlus, sigma_s, sigma_r, currentTreeLevel+1));
}
return true;
}
Eigen::MatrixXf CoMIK::getJacobianOfCoM(RobotNodePtr node)
{
// Get number of degrees of freedom
size_t nDoF = rns->getAllRobotNodes().size();
// Create matrices for the position and the orientation part of the jacobian.
Eigen::MatrixXf position = Eigen::MatrixXf::Zero(3, nDoF);
const std::vector<RobotNodePtr> parentsN = bodyNodeParents[node];
// Iterate over all degrees of freedom
for (size_t i = 0; i < nDoF; i++)
{
RobotNodePtr dof = rns->getNode(i);// bodyNodes[i];
// Check if the tcp is affected by this DOF
if (find(parentsN.begin(), parentsN.end(), dof) != parentsN.end())
{
// Calculus for rotational joints is different as for prismatic joints.
if (dof->isRotationalJoint())
{
// get axis
boost::shared_ptr<RobotNodeRevolute> revolute
= boost::dynamic_pointer_cast<RobotNodeRevolute>(dof);
THROW_VR_EXCEPTION_IF(!revolute, "Internal error: expecting revolute joint");
// todo: find a better way of handling different joint types
Eigen::Vector3f axis = revolute->getJointRotationAxis(coordSystem);
// For CoM-Jacobians only the positional part is necessary
Eigen::Vector3f toTCP = node->getCoMLocal() + node->getGlobalPose().block(0, 3, 3, 1)
- dof->getGlobalPose().block(0, 3, 3, 1);
position.block(0, i, 3, 1) = axis.cross(toTCP);
}
else if (dof->isTranslationalJoint())
{
// -> prismatic joint
boost::shared_ptr<RobotNodePrismatic> prismatic
= boost::dynamic_pointer_cast<RobotNodePrismatic>(dof);
THROW_VR_EXCEPTION_IF(!prismatic, "Internal error: expecting prismatic joint");
// todo: find a better way of handling different joint types
Eigen::Vector3f axis = prismatic->getJointTranslationDirection(coordSystem);
position.block(0, i, 3, 1) = axis;
}
}
}
if (target.rows() == 2)
{
Eigen::MatrixXf result(2, nDoF);
result.row(0) = position.row(0);
result.row(1) = position.row(1);
return result;
}
else if (target.rows() == 1)
{
VR_INFO << "One dimensional CoMs not supported." << endl;
}
return position;
}
void KinematicGraph::projectConfigurationSpace(int reducedDOFs,std::vector<double> stamps, KinematicParams ¶ms,KinematicData &data) {
if(reducedDOFs == 0) return;
if(stamps.size() == 0) return;
map< GenericModelPtr, int> DOFoffsets;
int DOFs = 0;
for(KinematicGraph::iterator it=begin(); it != end();it++) {
DOFoffsets[it->second] = DOFs;
DOFs += it->second->getDOFs();
}
if(DOFs==0) return;
MatrixXf X_trans(stamps.size(), DOFs);
for(KinematicGraph::iterator it=begin(); it != end();it++) {
GenericModelPtr model = it->second;
int DOFoffset = DOFoffsets[model];
if(model->getDOFs()==0) continue;
for(size_t timestamp_idx = 0;timestamp_idx < stamps.size();timestamp_idx++) {
int idx = data.poseIndex[ it->first.first ][it->first.second][ stamps[timestamp_idx]];
VectorXd c = model->getConfiguration(idx);
for(size_t d=0;d< model->getDOFs();d++) {
X_trans( timestamp_idx,DOFoffset + d ) = c(d);
}
}
}
VectorXf X_mean = VectorXf::Zero(DOFs,1);
for( size_t i=0;i< stamps.size();i++) {
for( int j=0;j< DOFs;j++) {
X_mean(j) += X_trans(i,j);
}
}
X_mean /= stamps.size();
// check for nans or large numbers (crash eigen while svd'ing)
for( int j=0;j< DOFs;j++) {
if(isnan((double)X_mean(j))) {
cout << X_trans << endl<<endl;
cout << X_mean << endl;
cout <<"reducing DOFs failed, NaN value"<<endl;
return;
}
if(fabs((double)X_mean(j))>100) {
cout << X_trans << endl<<endl;
cout << X_mean << endl;
cout <<"reducing DOFs failed, large value"<<endl;
return;
}
}
MatrixXf X_center(stamps.size(), DOFs);
for( size_t i=0;i< stamps.size();i++) {
for( int j=0;j< DOFs;j++) {
X_center(i,j) = X_trans(i,j) - X_mean(j);
}
}
if(stamps.size() < (size_t)DOFs) return;
JacobiSVD<MatrixXf> svdOfA(X_center);
const Eigen::MatrixXf U = svdOfA.matrixU();
const Eigen::MatrixXf V = svdOfA.matrixV();
const Eigen::VectorXf S = svdOfA.singularValues();
//DiagonalMatrix<Eigen::VectorXf> S2(S);
Eigen::MatrixXf V_projection = V.block(0,0,DOFs,reducedDOFs);
MatrixXf X_reduced(stamps.size(),reducedDOFs);
for( size_t i=0;i< stamps.size();i++) {
for( int k=0;k< reducedDOFs;k++) {
X_reduced(i,k) = 0.0;
for( int j=0;j< DOFs;j++) {
X_reduced(i,k) += V(j,k) * X_center(i,j);
}
}
}
MatrixXf X_projected(stamps.size(),DOFs);
for( size_t i=0;i< stamps.size();i++) {
for( int j=0;j< DOFs;j++) {
X_projected(i,j) = X_mean(j);
for( int k=0;k< reducedDOFs;k++) {
X_projected(i,j) += V(j,k) * X_reduced(i,k);
}
}
}
for(KinematicGraph::iterator it=begin(); it != end();it++) {
GenericModelPtr model = it->second;
if(model->getDOFs()==0) continue;
int DOFoffset = DOFoffsets[model];
for(size_t timestamp_idx = 0;timestamp_idx < stamps.size();timestamp_idx++) {
int idx = data.poseIndex[ it->first.first ][it->first.second][ stamps[timestamp_idx]];
VectorXd c(model->getDOFs());
for(size_t d=0;d< model->getDOFs();d++) {
c(d) =X_projected( timestamp_idx,DOFoffset + d );
}
model->setConfiguration(idx,c);
model->model.track.pose_projected[idx] = model->predictPose( c );
}
}
DOF = reducedDOFs;
}