void NonRigid::computeInflateDir(VectorXf &p_vec, VectorXf &g_vec)
{
VectorXf arap_g = g_vec;
VectorXf inflate_g = g_vec;
VectorXf userCrsp_g = g_vec;
double f_e = 0;
computeArap(p_vec, arap_g);
computeUserCrsp(p_vec, userCrsp_g);
Vector3f cur_v(0,0,0);
Vector3f cur_g(0,0,0);
size_t P_Num = p_vec.size()/3;
for (size_t i = 0; i < P_Num; ++i)
{
cur_v << p_vec(i + 0*P_Num), p_vec(i + 1*P_Num), p_vec(i + 2*P_Num);
if (computeVertexGrad(cur_v, cur_g) > 0.0)
{
inflate_g(i + 0*P_Num) = cur_g(0);
inflate_g(i + 1*P_Num) = cur_g(1);
inflate_g(i + 2*P_Num) = cur_g(2);
}
else
{
inflate_g(i + 0*P_Num) = -P_Prime_N(0, i);
inflate_g(i + 1*P_Num) = -P_Prime_N(1, i);
inflate_g(i + 2*P_Num) = -P_Prime_N(2, i);
}
}
g_vec = lamd_inflate*inflate_g + lamd_arap*arap_g + lamd_userCrsp*userCrsp_g;
g_vec.normalize();
}
double NonRigid::computeGradEnergy(VectorXf &p_vec, VectorXf &g_vec)
{
VectorXf arap_g = g_vec;
VectorXf dist_g = g_vec;
VectorXf userCrsp_g = g_vec;
double f_e = 0;
computeArap(p_vec, arap_g);
f_e += computeDistEnergy(p_vec, dist_g);
computeUserCrsp(p_vec, userCrsp_g);
g_vec = lamd_dist*dist_g + lamd_arap*arap_g + lamd_userCrsp*userCrsp_g;
g_vec.normalize();
return f_e;
}
bool Energy::iterate() {
if (step_size <= step_size_end) { return true; }
step_size = update_step_size(step_size, step_size_end);
if (step_size <= step_size_end) { return true; }
// every coordinate of a vector will all equal coordinates and a norm of
// step_size has the following value:
float dparam = step_size;
float energy_now = calc_energy();
VectorXf *diff = new VectorXf(numparams);
VectorXf *chg = new VectorXf(numparams);
for (int np = 0; np < numparams; np++) {
(*diff)[np] = 0.0f;
}
bool *done = new bool[numparams];
for (int np = 0; np < numparams; np++) {
(*chg )[np] = 0;
done[np] = false;
(*diff)[np] = dparam;
apply_change(diff);
float energy_large = calc_energy();
(*diff)[np] = -2 * dparam;
apply_change(diff);
float energy_small = calc_energy();
(*diff)[np] = dparam;
apply_change(diff);
(*diff)[np] = 0;
if (!boost::math::isnan(energy_small) &&
!boost::math::isnan(energy_large)) {
if (!is_improvement(energy_now, energy_large) &&
!is_improvement(energy_now, energy_small)) {
if (step_size <= step_size_end) { done[np] = true; }
continue;
}
(*chg)[np] = energy_small - energy_large;
}
else {
(*chg)[np] = 0.0f;
}
}
bool chg_all_zeros = true;
for (int np = 0; np < numparams; np++) {
chg_all_zeros = chg_all_zeros && ((*chg)[np] == 0.0f);
}
if (!chg_all_zeros) {
chg->normalize();
(*chg) *= step_size * speed;
apply_change(chg);
float energy_new = calc_energy();
if (!is_improvement(energy_now, energy_new) &&
step_size > step_size_end) {
(*chg) *= -1;
apply_change(chg);
step_size /= 2;
}
log_iteration(step_size);
}
else if (step_size > step_size_end) {
step_size /= 2;
}
log_energies();
delete chg ;
delete diff;
for (int np = 0; np < numparams; np++) {
if (!done[np]) { delete done; return false; }
}
delete done;
return true;
}
IplImage* CloudProjection::computeProjection(const sensor_msgs::PointCloud& data,
const std::vector<int>& interest_region_indices)
{
// -- Put cluster points into matrix form.
MatrixXf points(interest_region_indices.size(), 3);
for(size_t i=0; i<interest_region_indices.size(); ++i) {
points(i, 0) = data.points[interest_region_indices[i]].x;
points(i, 1) = data.points[interest_region_indices[i]].y;
points(i, 2) = data.points[interest_region_indices[i]].z;
}
// -- Subtract off the mean and flatten to z=0 to prepare for PCA.
MatrixXf X = points;
X.col(2) = VectorXf::Zero(X.rows());
VectorXf pt_mean = X.colwise().sum() / (float)X.rows();
for(int i=0; i<X.rows(); ++i) {
X.row(i) -= pt_mean.transpose();
}
MatrixXf Xt = X.transpose();
// -- Find the long axis.
// Start with a random vector.
VectorXf pc = VectorXf::Zero(3);
pc(0) = 1; //Chosen by fair dice roll.
pc(1) = 1;
pc.normalize();
// Power method.
VectorXf prev = pc;
double thresh = 1e-4;
int ctr = 0;
while(true) {
prev = pc;
pc = Xt * (X * pc);
pc.normalize();
ctr++;
if((pc - prev).norm() < thresh)
break;
}
assert(abs(pc(2)) < 1e-4);
// -- Find the short axis.
VectorXf shrt = VectorXf::Zero(3);
shrt(1) = -pc(0);
shrt(0) = pc(1);
assert(abs(shrt.norm() - 1) < 1e-4);
assert(abs(shrt.dot(pc)) < 1e-4);
// -- Build the basis of normalized coordinates.
MatrixXf basis = MatrixXf::Zero(3,3);
basis.col(0) = pc;
basis.col(1) = shrt;
basis(2,2) = -1.0;
assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
assert(abs(basis.col(0).norm() - 1) < 1e-4);
assert(abs(basis.col(1).norm() - 1) < 1e-4);
assert(abs(basis.col(2).norm() - 1) < 1e-4);
// -- Put the cluster into normalized coordinates, and choose which axis to project on.
MatrixXf projected_basis(3, 2);
if(axis_ == 0) {
projected_basis.col(0) = basis.col(1);
projected_basis.col(1) = basis.col(2);
}
else if(axis_ == 1) {
projected_basis.col(0) = basis.col(0);
projected_basis.col(1) = basis.col(2);
}
else if(axis_ == 2) {
projected_basis.col(0) = basis.col(0);
projected_basis.col(1) = basis.col(1);
}
MatrixXf projected = points * projected_basis;
// -- Transform into pixel units.
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) *= pixels_per_meter_;
projected(i, 1) *= pixels_per_meter_;
}
// -- Find min and max of u and v. TODO: noise sensitivity?
float min_v = FLT_MAX;
float min_u = FLT_MAX;
float max_v = -FLT_MAX;
float max_u = -FLT_MAX;
for(int i=0; i<projected.rows(); ++i) {
float u = projected(i, 0);
float v = projected(i, 1);
if(u < min_u)
min_u = u;
if(u > max_u)
max_u = u;
if(v < min_v)
min_v = v;
if(v > max_v)
max_v = v;
}
// -- Shift the origin based on {u,v}_offset_pct.
// u_offset_pct_ is the percent of the way from min_u to max_u that the
// u_offset should be set to. If this makes the window fall outside min_u or max_u,
// then shift the window so that it is inside.
float u_offset = u_offset_pct_ * (max_u - min_u) + min_u;
float v_offset = v_offset_pct_ * (max_v - min_v) + min_v;
if(u_offset_pct_ > 0.5 && u_offset + cols_ / 2 > max_u)
u_offset = max_u - cols_ / 2 + 1;
if(u_offset_pct_ < 0.5 && u_offset - cols_ / 2 < min_u)
u_offset = min_u + cols_ / 2 - 1;
if(v_offset_pct_ > 0.5 && v_offset + rows_ / 2 > max_v)
v_offset = max_v - rows_ / 2 + 1;
if(v_offset_pct_ < 0.5 && v_offset - rows_ / 2 < min_v)
v_offset = min_v + rows_ / 2 - 1;
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) -= u_offset - (float)cols_ / 2.0;
projected(i, 1) -= v_offset - (float)rows_ / 2.0;
}
// -- Fill the IplImages.
assert(sizeof(float) == 4);
IplImage* acc = cvCreateImage(cvSize(cols_, rows_), IPL_DEPTH_32F, 1);
IplImage* img = cvCreateImage(cvSize(cols_, rows_), IPL_DEPTH_32F, 1);
cvSetZero(acc);
cvSetZero(img);
for(int i=0; i<projected.rows(); ++i) {
int row = floor(projected(i, 1));
int col = floor(projected(i, 0));
if(row >= rows_ || col >= cols_ || row < 0 || col < 0)
continue;
float intensity = (float)data.channels[0].values[interest_region_indices[i]] / 255.0 * (3.0 / 4.0) + 0.25;
//cout << i << ": " << interest_region_indices[i] << "/" << data.channels[0].values.size() << " " << (float)data.channels[0].values[interest_region_indices[i]] << " " << intensity << endl;
assert(interest_region_indices[i] < (int)data.channels[0].values.size() && (int)interest_region_indices[i] >= 0);
assert(intensity <= 1.0 && intensity >= 0.0);
((float*)(img->imageData + row * img->widthStep))[col] += intensity;
((float*)(acc->imageData + row * acc->widthStep))[col]++;
}
// -- Normalize by the number of points falling in each pixel.
for(int v=0; v<rows_; ++v) {
float* img_ptr = (float*)(img->imageData + v * img->widthStep);
float* acc_ptr = (float*)(acc->imageData + v * acc->widthStep);
for(int u=0; u<cols_; ++u) {
if(*acc_ptr == 0)
*img_ptr = 0;
else
*img_ptr = *img_ptr / *acc_ptr;
img_ptr++;
acc_ptr++;
}
}
// -- Clean up and return.
cvReleaseImage(&acc);
return img;
}
MatrixXf D3DCloudOrienter::orientCloud(const sensor_msgs::PointCloud& data,
const std::vector<int>& interest_region_indices)
{
// -- Put cluster points into matrix form.
MatrixXf points(interest_region_indices.size(), 3);
for(size_t i=0; i<interest_region_indices.size(); ++i) {
points(i, 0) = data.points[interest_region_indices[i]].x;
points(i, 1) = data.points[interest_region_indices[i]].y;
points(i, 2) = data.points[interest_region_indices[i]].z;
}
// -- Subtract off the mean of the points.
VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
for(int i=0; i<points.rows(); ++i)
points.row(i) -= pt_mean.transpose();
// -- Flatten to z == 0.
MatrixXf X = points;
X.col(2) = VectorXf::Zero(X.rows());
MatrixXf Xt = X.transpose();
// -- Find the long axis.
// Start with a random vector.
VectorXf pc = VectorXf::Zero(3);
pc(0) = 1; //Chosen by fair dice roll.
pc(1) = 1;
pc.normalize();
// Power method.
VectorXf prev = pc;
double thresh = 1e-4;
int ctr = 0;
while(true) {
prev = pc;
pc = Xt * (X * pc);
pc.normalize();
ctr++;
if((pc - prev).norm() < thresh)
break;
}
assert(abs(pc(2)) < 1e-4);
// -- Find the short axis.
VectorXf shrt = VectorXf::Zero(3);
shrt(1) = -pc(0);
shrt(0) = pc(1);
assert(abs(shrt.norm() - 1) < 1e-4);
assert(abs(shrt.dot(pc)) < 1e-4);
// -- Build the basis of normalized coordinates.
MatrixXf basis = MatrixXf::Zero(3,3);
basis.col(0) = pc;
basis.col(1) = shrt;
basis(2,2) = 1.0;
assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
assert(abs(basis.col(0).norm() - 1) < 1e-4);
assert(abs(basis.col(1).norm() - 1) < 1e-4);
assert(abs(basis.col(2).norm() - 1) < 1e-4);
// -- Rotate and return.
MatrixXf oriented = points * basis;
return oriented;
}
void CloudOrienter::_compute() {
assert(input_cloud_);
assert(input_intensities_);
assert(!output_cloud_);
//cout << input_intensities_->rows() << " " << input_cloud_->rows() << endl;
assert(input_cloud_->rows() == input_intensities_->rows());
assert(input_cloud_->rows() > 2);
// -- Subtract off the mean of the points.
MatrixXf& points = *input_cloud_;
VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
for(int i=0; i<points.rows(); ++i)
points.row(i) -= pt_mean.transpose();
// -- Flatten to z == 0.
MatrixXf X = points;
X.col(2) = VectorXf::Zero(X.rows());
MatrixXf Xt = X.transpose();
// -- Find the long axis.
// Start with a random vector.
VectorXf pc = VectorXf::Zero(3);
pc(0) = 1; //Chosen by fair dice roll.
pc(1) = 1;
pc.normalize();
// Power method.
VectorXf prev = pc;
double thresh = 1e-4;
int ctr = 0;
while(true) {
prev = pc;
pc = Xt * (X * pc);
pc.normalize();
ctr++;
if((pc - prev).norm() < thresh)
break;
// -- In some degenerate cases, it is possible for the vector
// to never settle down to the first PC.
if(ctr > 100)
break;
}
assert(abs(pc(2)) < 1e-4);
// -- Find the short axis.
VectorXf shrt = VectorXf::Zero(3);
shrt(1) = -pc(0);
shrt(0) = pc(1);
assert(abs(shrt.norm() - 1) < 1e-4);
assert(abs(shrt.dot(pc)) < 1e-4);
// -- Build the basis of normalized coordinates.
MatrixXf basis = MatrixXf::Zero(3,3);
basis.col(0) = pc;
basis.col(1) = shrt;
basis(2,2) = 1.0;
assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
assert(abs(basis.col(0).norm() - 1) < 1e-4);
assert(abs(basis.col(1).norm() - 1) < 1e-4);
assert(abs(basis.col(2).norm() - 1) < 1e-4);
// -- Rotate and set the output_cloud_.
output_cloud_ = shared_ptr<MatrixXf>(new MatrixXf);
*output_cloud_ = points * basis;
assert(output_cloud_->rows() == input_cloud_->rows());
}
