void BorderMattingHandler::constructTrimap(){
Point p;
double w = 6.0;
pixelidx.clear();
pixelidx.resize(contours.size());
//traversal all pixel.
cout<<"constructing trimap."<<endl;
for (p.y = 0; p.y < _img.rows; p.y++) {
for (p.x = 0; p.x < _img.cols; p.x++) {
int id = minestDistanceContourIndex(p);
if (contours[id].dist(p) < w) {
pixelidx[id].push_back(p);
trimap.at<uchar>(p) = BM_U;
}
}
}
cout<<"finished."<<endl;
imshow("trimap", trimap);
//watchpixelidx();
setupGauss();
initSolve();
solveEnergyFunction();
computeAlpha();
rejectOutlier();
imshow("alpha", alphamap);
}
void
LinearIsotropicMaterial::computeProperties()
{
for (_qp = 0; _qp < _qrule->n_points(); ++_qp)
{
Real alpha = computeAlpha();
_local_elasticity_tensor->calculate(_qp);
_elasticity_tensor[_qp] = *_local_elasticity_tensor;
SymmTensor strn(_grad_disp_x[_qp](0),
_grad_disp_y[_qp](1),
_grad_disp_z[_qp](2),
0.5 * (_grad_disp_x[_qp](1) + _grad_disp_y[_qp](0)),
0.5 * (_grad_disp_y[_qp](2) + _grad_disp_z[_qp](1)),
0.5 * (_grad_disp_z[_qp](0) + _grad_disp_x[_qp](2)));
// Add in Isotropic Thermal Strain
if (_has_temp)
{
Real isotropic_strain = alpha * (_temp[_qp] - _t_ref);
strn.addDiag(-isotropic_strain);
_d_strain_dT.zero();
_d_strain_dT.addDiag(-alpha);
}
SymmTensor strain(strn);
computeStress(strain, _stress[_qp]);
}
}
/**
* This function computes two-dimensional lighting textures which can
* be used for illuminating lines.
*
* ka The ambient reflection coefficient.环境系数
* kd The diffuse reflection coefficient.反射系数
* ks The specular reflection coefficient.镜面发射系数
* n The gloss exponent for the specular lighting.
* texDim The dimension of the square matrices.
* tex Address where to store the texture for the
* full lighting.
* If NULL, this texture will not be computed.
* texDiff Address where to store the texture for the ambient and
* diffuse lighting.
* If NULL, this texture will not be computed.
* texSpec Address where to store the texture for the specular
* lighting.
* If NULL, this texture will not be computed.
* lightingModel The lighting model to use.
* stretch Flag whether to stretch the dynamic range.
* L The normalized light vector.
* Only required for the ILLightingModel::IL_CYLINDER_PHONG
* lighting model.
* @param V The normalized viewing vector.
* Only required for the ILLightingModel::IL_CYLINDER_PHONG
* lighting model.
*/
void computeTextures(float ka, float kd, float ks, float n,
int texDim,
float *tex, float *texDiff, float *texSpec,
bool stretch,
const float *L, const float *V)
{
float *iter, *iterDiff, *iterSpec;
double s, t;
double LT, VT, LV;
double alpha;
double diffTerm, specTerm;
double stretchDiff, stretchSpec;
iter = tex;
iterDiff = texDiff;
iterSpec = texSpec;
stretchDiff = stretchSpec = 1.0;
if (stretch)
{
stretchDiff = computeStretchFactorDiff();
}
for (int y = 0; y < texDim; y++)
for (int x = 0; x < texDim; x++)
{
/* Avoid divisions by zero. */
s = ((double)x + 0.5) / (double)texDim;
t = ((double)y + 0.5) / (double)texDim;
LT = 2.0 * s - 1.0;
VT = 2.0 * t - 1.0;
LV = dot(L, V);
alpha = computeAlpha(LV, LT, VT);
diffTerm = stretchDiff * computeDiffTerm(LT, alpha);
specTerm = computeSpecTermPhong(LT, VT, alpha, n);
//}
//printf("%f\t%f\t%f\n",diffTerm,specTerm,alpha);
//getchar();
if (iter != NULL)
*iter++ = (float)(ka + kd * diffTerm + ks * specTerm);
if (iterDiff != NULL)
*iterDiff++ = (float)(ka + kd * diffTerm);
if (iterSpec != NULL)
*iterSpec++ = (float)(ks * specTerm);
//printf("%f\t%f\t%f\n",*iter,*iterDiff,*iterSpec);
}
}
void computeCurrentPositionUsingCoders(PidMotion* pidMotion) {
PidComputationValues* computationValues = &(pidMotion->computationValues);
PidCurrentValues* thetaCurrentValues = &(computationValues->currentValues[THETA]);
PidCurrentValues* alphaCurrentValues = &(computationValues->currentValues[ALPHA]);
// 2 dependant Wheels (direction + angle)
float value0 = (float)getCoderValue(CODER_LEFT);
float value1 = (float)getCoderValue(CODER_RIGHT);
// Compute real position of wheel
thetaCurrentValues->position = computeTheta(value0, value1);
alphaCurrentValues->position = computeAlpha(value0, value1);
}
void Disp2DPropLoadEngine::letDisturb()
{
const Real& dt = scene->dt;
dgamma=cos(theta*Mathr::PI/180.0)*v*dt;
dh=sin(theta*Mathr::PI/180.0)*v*dt;
Real Ysup = topbox->state->pos.y();
Real Ylat = leftbox->state->pos.y();
// Changes in vertical and horizontal position :
topbox->state->pos += Vector3r(dgamma,dh,0);
leftbox->state->pos += Vector3r(dgamma/2.0,dh/2.0,0);
rightbox->state->pos += Vector3r(dgamma/2.0,dh/2.0,0);
Real Ysup_mod = topbox->state->pos.y();
Real Ylat_mod = leftbox->state->pos.y();
// with the corresponding velocities :
topbox->state->vel=Vector3r(dgamma/dt,dh/dt,0);
leftbox->state->vel = Vector3r((dgamma/dt)/2.0,dh/(2.0*dt),0);
rightbox->state->vel = Vector3r((dgamma/dt)/2.0,dh/(2.0*dt),0);
// Then computation of the angle of the rotation to be done :
computeAlpha();
if (alpha == Mathr::PI/2.0) // Case of the very beginning
{
dalpha = - atan( dgamma / (Ysup_mod -Ylat_mod) );
}
else
{
Real A = (Ysup_mod - Ylat_mod) * 2.0*tan(alpha) / (2.0*(Ysup - Ylat) + dgamma*tan(alpha) );
dalpha = atan( (A - tan(alpha))/(1.0 + A * tan(alpha)));
}
Quaternionr qcorr(AngleAxisr(dalpha,Vector3r::UnitZ()));
if(LOG)
cout << "Quaternion associe a la rotation incrementale : " << qcorr.w() << " " << qcorr.x() << " " << qcorr.y() << " " << qcorr.z() << endl;
// On applique la rotation en changeant l'orientation des plaques, leurs vang et en affectant donc alpha
leftbox->state->ori = qcorr*leftbox->state->ori;
leftbox->state->angVel = Vector3r(0,0,1)*dalpha/dt;
rightbox->state->ori = qcorr*leftbox->state->ori;
rightbox->state->angVel = Vector3r(0,0,1)*dalpha/dt;
}
void
LinearIsotropicMaterial::computeProperties()
{
for (_qp=0; _qp < _qrule->n_points(); ++_qp)
{
Real alpha = computeAlpha();
_local_elasticity_tensor->calculate(_qp);
_elasticity_tensor[_qp] = *_local_elasticity_tensor;
SymmTensor strn( _grad_disp_x[_qp](0),
_grad_disp_y[_qp](1),
_grad_disp_z[_qp](2),
0.5*(_grad_disp_x[_qp](1)+_grad_disp_y[_qp](0)),
0.5*(_grad_disp_y[_qp](2)+_grad_disp_z[_qp](1)),
0.5*(_grad_disp_z[_qp](0)+_grad_disp_x[_qp](2)) );
// Add in Isotropic Thermal Strain
if (_has_temp)
{
Real isotropic_strain = alpha * (_temp[_qp] - _t_ref);
strn.addDiag( -isotropic_strain );
_d_strain_dT.zero();
_d_strain_dT.addDiag( -alpha );
}
SymmTensor v_strain(0);
SymmTensor dv_strain_dT(0);
for (unsigned int i(0); i < _volumetric_models.size(); ++i)
{
_volumetric_models[i]->modifyStrain(_qp, 1, v_strain, dv_strain_dT);
}
SymmTensor strain( v_strain );
strain *= _dt;
strain += strn;
dv_strain_dT *= _dt;
_d_strain_dT += dv_strain_dT;
computeStress(strain, _stress[_qp]);
}
}
/**
* Go to a position
*/
void gotoSimplePosition(PidMotion* pidMotion, float leftMM, float rightMM, float a, float speed, OutputStream* notificationOutputStream) {
// determine the type of motion
enum MotionParameterType motionParameterType = getMotionParameterType(leftMM, rightMM);
// Alpha / Theta
float thetaNextPosition = computeTheta(leftMM, rightMM);
float alphaNextPosition = computeAlpha(leftMM, rightMM);
PidMotionDefinition* motionDefinition = pidMotionGetNextToWritePidMotionDefinition(pidMotion);
motionDefinition->motionType = MOTION_TYPE_NORMAL;
motionDefinition->notificationOutputStream = notificationOutputStream;
setNextPosition(motionDefinition, THETA, motionParameterType, thetaNextPosition, a, speed);
setNextPosition(motionDefinition, ALPHA, motionParameterType, alphaNextPosition, a, speed);
// All main information are defined
motionDefinition->state = PID_MOTION_DEFINITION_STATE_SET;
}
ZMPDistributor::ForceTorque ZMPDistributor::distributeZMP(const Eigen::Vector3f& localAnkleLeft,
const Eigen::Vector3f& localAnkleRight,
const Eigen::Matrix4f& leftFootPoseGroundFrame,
const Eigen::Matrix4f& rightFootPoseGroundFrame,
const Eigen::Vector3f& zmpRefGroundFrame,
Bipedal::SupportPhase phase)
{
Eigen::Matrix4f groundPoseLeft = Bipedal::projectPoseToGround(leftFootPoseGroundFrame);
Eigen::Matrix4f groundPoseRight = Bipedal::projectPoseToGround(rightFootPoseGroundFrame);
Eigen::Vector3f localZMPLeft = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseLeft.inverse());
Eigen::Vector3f localZMPRight = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseRight.inverse());
double alpha = computeAlpha(groundPoseLeft, groundPoseRight, zmpRefGroundFrame, localZMPLeft.head(2), localZMPRight.head(2), phase);
//std::cout << "########## " << alpha << " ###########" << std::endl;
ForceTorque ft;
// kg*m/s^2 = N
ft.leftForce = -(1-alpha) * mass * gravity;
ft.rightForce = -alpha * mass * gravity;
// Note we need force as kg*mm/s^2
ft.leftTorque = (localAnkleLeft - localZMPLeft).cross(ft.leftForce * 1000);
ft.rightTorque = (localAnkleRight - localZMPRight).cross(ft.rightForce * 1000);
// ZMP not contained in convex polygone
if (std::fabs(alpha) > std::numeric_limits<float>::epsilon()
&& std::fabs(alpha-1) > std::numeric_limits<float>::epsilon())
{
Eigen::Vector3f leftTorqueGroundFrame = groundPoseLeft.block(0, 0, 3, 3) * ft.leftTorque;
Eigen::Vector3f rightTorqueGroundFrame = groundPoseRight.block(0, 0, 3, 3) * ft.rightTorque;
Eigen::Vector3f tau0 = -1 * (leftTorqueGroundFrame + rightTorqueGroundFrame);
//std::cout << "Tau0World: " << tau0.transpose() << std::endl;
//std::cout << "leftTorqueWorld: " << leftTorqueWorld.transpose() << std::endl;
//std::cout << "rightTorqueWorld: " << rightTorqueWorld.transpose() << std::endl;
// Note: Our coordinate system is different than in the paper!
// Also this is not the same as the ground frame.
Eigen::Vector3f xAxis = leftFootPoseGroundFrame.block(0,3,3,1) + localAnkleLeft
- localAnkleRight - rightFootPoseGroundFrame.block(0,3,3,1);
xAxis /= xAxis.norm();
Eigen::Vector3f zAxis(0, 0, 1);
Eigen::Vector3f yAxis = zAxis.cross(xAxis);
yAxis /= yAxis.norm();
Eigen::Matrix3f centerFrame;
centerFrame.block(0, 0, 3, 1) = xAxis;
centerFrame.block(0, 1, 3, 1) = yAxis;
centerFrame.block(0, 2, 3, 1) = zAxis;
//std::cout << "Center frame:\n" << centerFrame << std::endl;
Eigen::Vector3f centerTau0 = centerFrame.transpose() * tau0;
Eigen::Vector3f leftTorqueCenter;
leftTorqueCenter.x() = (1-alpha)*centerTau0.x();
leftTorqueCenter.y() = centerTau0.y() < 0 ? centerTau0.y() : 0;
leftTorqueCenter.z() = 0;
Eigen::Vector3f rightTorqueCenter;
rightTorqueCenter.x() = alpha*centerTau0.x();
rightTorqueCenter.y() = centerTau0.y() < 0 ? 0 : centerTau0.y();
rightTorqueCenter.z() = 0;
//std::cout << "Tau0Center: " << centerTau0.transpose() << std::endl;
//std::cout << "leftTorqueCenter: " << leftTorqueCenter.transpose() << std::endl;
//std::cout << "rightTorqueCenter: " << rightTorqueCenter.transpose() << std::endl;
// tcp <--- ground frame <--- center frame
ft.leftTorque = groundPoseLeft.block(0, 0, 3, 3).transpose() * centerFrame * leftTorqueCenter;
ft.rightTorque = groundPoseRight.block(0, 0, 3, 3).transpose() * centerFrame * rightTorqueCenter;
}
// Torque depends on timestep, we need a way to do this correctly.
const double torqueFactor = 1;
// convert to Nm
ft.leftTorque *= torqueFactor / 1000.0 / 1000.0;
ft.rightTorque *= torqueFactor / 1000.0 / 1000.0;
//std::cout << "leftTorque: " << ft.leftTorque.transpose() << std::endl;
//std::cout << "rightTorque: " << ft.rightTorque.transpose() << std::endl;
return ft;
}
void GRID::computeAlphaStretchRight(int c) {
double Dx = rmax[c] - rmaxUniformGrid[c];
double Dxi = NRightStretcheGrid[c] * dr[c];
rightAlphaStretch[c] = computeAlpha(Dx, Dxi);
}
void GRID::computeAlphaStretchLeft(int c) {
double Dx = rminUniformGrid[c] - rmin[c];
double Dxi = NLeftStretcheGrid[c] * dr[c];
leftAlphaStretch[c] = computeAlpha(Dx, Dxi);
}
