/* ==================== TO IMPLEMENT =======================
* measurement(0) : x-position of marker in drone's xy-coordinate system (independant of roll, pitch)
* measurement(1): y-position of marker in drone's xy-coordinate system (independant of roll, pitch)
* measurement(2): yaw rotation of marker, in drone's xy-coordinate system (independant of roll, pitch)
*
* global_marker_pose(0): x-position or marker in world-coordinate system
* global_marker_pose(1): y-position or marker in world-coordinate system
* global_marker_pose(2): yaw-rotation or marker in world-coordinate system
*/
void ExtendedKalmanFilter::correctionStep(const Eigen::Vector3f& measurement, const Eigen::Vector3f& global_marker_pose)
{
printf("TO IMPLEMENT! ekf: %f %f %f; obs: %f %f %f\n",
state[0],state[1],state[2],
measurement[0], measurement[1], measurement[2]);
float dx = global_marker_pose(0)-state(0); //dx = x_marker - x_drone_global
float dy = global_marker_pose(1)-state(1);//dy = y_marker - y_drone_global
float dYaw = global_marker_pose(2)-state(2);//dYaw = yaw_marker - yaw_drone_global
// dh/dx:
Eigen::Matrix3f H;
H << -cos(state(2)), -sin(state(2)), -dx*sin(state(2))+dy*cos(state(2)),
sin(state(2)), -cos(state(2)), -dx*cos(state(2))-dy*sin(state(2)),
0, 0, -1;
//K
Eigen::Matrix3f K, L;
L = H*sigma*H.transpose()+R;
K = sigma*H.transpose()*L.inverse();
//update state
//f = z-h(x)
Eigen::Vector3f f;
f << measurement(0)-cos(state(2))*dx-sin(state(2))*dy,
measurement(1)+sin(state(2))*dx-cos(state(2))*dy,
measurement(2)-dYaw;
f(2) = atan2(sin(f(2)),cos(f(2))); // normalize angle
state = state + K*f;
Eigen::Vector3f a;
a = K*f;
cout << "f: " << f << endl;
cout << "K: " << K << endl;
cout << "K*f" << a << endl;
//update covariance matrix
Eigen::Matrix3f I;
I << 1,0,0,
0,1,0,
0,0,1;
sigma = (I - K*H)*sigma;
}
void CCubicGrids::extractRTFromBuffer(const cuda::GpuMat& cvgmSumBuf_, Eigen::Matrix3f* peimRw_, Eigen::Vector3f* peivTw_) const{
Mat cvmSumBuf;
cvgmSumBuf_.download(cvmSumBuf);
double* aHostTmp = (double*)cvmSumBuf.data;
//declare A and b
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
Eigen::Matrix<double, 6, 1> b;
//retrieve A and b from cvmSumBuf
short sShift = 0;
for (int i = 0; i < 6; ++i){ // rows
for (int j = i; j < 7; ++j) { // cols + b
double value = aHostTmp[sShift++];
if (j == 6) // vector b
b.data()[i] = value;
else
A.data()[j * 6 + i] = A.data()[i * 6 + j] = value;
}//for each col
}//for each row
//checking nullspace
double dDet = A.determinant();
if (fabs(dDet) < 1e-15 || dDet != dDet){
if (dDet != dDet)
PRINTSTR("Failure -- dDet cannot be qnan. ");
//reset ();
return;
}//if dDet is rational
//float maxc = A.maxCoeff();
Eigen::Matrix<float, 6, 1> result = A.llt().solve(b).cast<float>();
//Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
float alpha = result(0);
float beta = result(1);
float gamma = result(2);
Eigen::Matrix3f Rinc = (Eigen::Matrix3f)Eigen::AngleAxisf(gamma, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(beta, Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX());
Eigen::Vector3f tinc = result.tail<3>();
//compose
//eivTwCur = Rinc * eivTwCur + tinc;
//eimrmRwCur = Rinc * eimrmRwCur;
Eigen::Vector3f eivTinv = -peimRw_->transpose()* (*peivTw_);
Eigen::Matrix3f eimRinv = peimRw_->transpose();
eivTinv = Rinc * eivTinv + tinc;
eimRinv = Rinc * eimRinv;
*peivTw_ = -eimRinv.transpose() * eivTinv;
*peimRw_ = eimRinv.transpose();
}
void createSymmetricProblems
(
size_t nr_problems,
Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
Vcl::Core::InterleavedArray<float, 3, 3, -1>* R
)
{
// Random number generator
std::mt19937_64 rng;
std::uniform_real_distribution<float> d;
for (int i = 0; i < (int)nr_problems; i++)
{
// Rest-state
Eigen::Matrix3f M;
M << d(rng), d(rng), d(rng),
d(rng), d(rng), d(rng),
d(rng), d(rng), d(rng);
Eigen::Matrix3f MtM = M.transpose() * M;
F.at<float>(i) = MtM;
if (R)
{
Eigen::Matrix3f Rot;
Vcl::Mathematics::PolarDecomposition(MtM, Rot, nullptr);
R->template at<float>(i) = Rot;
}
}
}
void
pcl::getCameraMatrixFromProjectionMatrix (
const Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix,
Eigen::Matrix3f& camera_matrix)
{
Eigen::Matrix3f KR = projection_matrix.topLeftCorner <3, 3> ();
Eigen::Matrix3f KR_KRT = KR * KR.transpose ();
Eigen::Matrix3f cam = KR_KRT / KR_KRT.coeff (8);
memset (&(camera_matrix.coeffRef (0)), 0, sizeof (Eigen::Matrix3f::Scalar) * 9);
camera_matrix.coeffRef (8) = 1.0;
if (camera_matrix.Flags & Eigen::RowMajorBit)
{
camera_matrix.coeffRef (2) = cam.coeff (2);
camera_matrix.coeffRef (5) = cam.coeff (5);
camera_matrix.coeffRef (4) = static_cast<float> (std::sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5)));
camera_matrix.coeffRef (1) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
camera_matrix.coeffRef (0) = static_cast<float> (std::sqrt (cam.coeff (0) - camera_matrix.coeff (1) * camera_matrix.coeff (1) - cam.coeff (2) * cam.coeff (2)));
}
else
{
camera_matrix.coeffRef (6) = cam.coeff (2);
camera_matrix.coeffRef (7) = cam.coeff (5);
camera_matrix.coeffRef (4) = static_cast<float> (std::sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5)));
camera_matrix.coeffRef (3) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
camera_matrix.coeffRef (0) = static_cast<float> (std::sqrt (cam.coeff (0) - camera_matrix.coeff (3) * camera_matrix.coeff (3) - cam.coeff (2) * cam.coeff (2)));
}
}
void ShapeMatch::setTarget(std::vector<double> &target_vec)
{
target_mat = Eigen::Map<Eigen::Matrix3Xd>(&target_vec[0], 3, target_vec.size()/3);
target_center = Eigen::Vector3d(target_mat.row(0).mean(), target_mat.row(0).mean(), target_mat.row(0).mean());
target_mat.row(0) = (target_mat.row(0).array() - target_center(0)).matrix();
target_mat.row(1) = (target_mat.row(1).array() - target_center(1)).matrix();
target_mat.row(2) = (target_mat.row(2).array() - target_center(2)).matrix();
// do icp alignment first
//Eigen::MatrixXd C_n = Eigen::MatrixXd::Identity(target_mat.cols(), target_mat.cols()) - Eigen::MatrixXd::Ones(target_mat.cols(), target_mat.cols()) / target_mat.cols();
Eigen::Matrix3d H = target_mat*template_mat.transpose();
Eigen::Matrix3f H_float = H.cast<float>();
Eigen::Matrix3f Ui;
Eigen::Vector3f Wi;
Eigen::Matrix3f Vi;
wunderSVD3x3(H_float, Ui, Wi, Vi);
Eigen::Matrix3d R = (Vi * Ui.transpose()).cast<double>();
Eigen::Vector3d t = template_center - R*target_center;
target_mat = R*target_mat + t*Eigen::MatrixXd::Ones(1, target_mat.cols());
target_center = Eigen::Vector3d(target_mat.row(0).mean(), target_mat.row(0).mean(), target_mat.row(0).mean());
target_mat.row(0) = (target_mat.row(0).array() - target_center(0)).matrix();
target_mat.row(1) = (target_mat.row(1).array() - target_center(1)).matrix();
target_mat.row(2) = (target_mat.row(2).array() - target_center(2)).matrix();
target_vec = std::vector<double>(target_mat.data(), target_mat.data()+target_mat.cols()*target_mat.rows());
}
/** Get the inliers among all matches that comply with a given fundamental matrix.
* @param std::vector<Match> vector with feature matches
* @param Eigen::Matrix3f fundamental matrix
* @return std::vector<int> vector with indices of the inliers */
std::vector<int> MonoOdometer5::getInliers(std::vector<Match> matches, Eigen::Matrix3f F)
{
std::vector<int> inlierIndices;
for(int i=0; i<matches.size(); i++)
{
cv::Point2f pPrev = matches[i].pPrev_;
cv::Point2f pCurr = matches[i].pCurr_;
Eigen::Vector3f pPrevHomog, pCurrHomog;
pPrevHomog << pPrev.x, pPrev.y, 1.0;
pCurrHomog << pCurr.x, pCurr.y, 1.0;
// xCurr^T * F * xPrev
double x2tFx1 = pCurrHomog.transpose() * F * pPrevHomog;
// F * xPrev
Eigen::Vector3f Fx1 = F * pPrevHomog;
// F^T * xCurr
Eigen::Vector3f Ftx2 = F.transpose() * pCurrHomog;
// compute Sampson distance (distance to epipolar line)
double dSampson = (x2tFx1 * x2tFx1) / ((Fx1(0)*Fx1(0)) + (Fx1(1)*Fx1(1)) + (Ftx2(0)*Ftx2(0)) + (Ftx2(1)*Ftx2(1)));
if(dSampson < param_odometerInlierThreshold_)
{
inlierIndices.push_back(i);
}
}
return inlierIndices;
}
void PairwiseRegistrationDialog::showResults(const Eigen::Matrix4f &transformation, float rmsError, int corrNumber)
{
std::stringstream temp;
Eigen::Matrix4f T = transformation;
temp.str("");
temp << T;
TTextEdit->setText(QString::fromStdString(temp.str()));
Eigen::Matrix3f R = T.block(0,0,3,3);
float c = (R * R.transpose()).diagonal().mean();
c = qSqrt(c);
cLineEdit->setText(QString::number(c));
R /= c;
temp.str("");
temp << R;
RTextEdit->setText(QString::fromStdString(temp.str()));
Eigen::Vector3f t = T.block(0,3,3,1);
temp.str("");
temp << t;
tLineEdit->setText(QString::fromStdString(temp.str()));
Eigen::AngleAxisf angleAxis(R);
angleLineEdit->setText( QString::number(angleAxis.angle()) );
temp.str("");
temp << angleAxis.axis();
axisLineEdit->setText(QString::fromStdString(temp.str()));
corrNumberLineEdit->setText(QString::number(corrNumber));
errorLineEdit->setText(QString::number(rmsError));
}
template <typename PointSource, typename PointTarget> void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::getTransformationFromCorrelation (
const Eigen::MatrixXf &cloud_src_demean,
const Eigen::Vector4f ¢roid_src,
const Eigen::MatrixXf &cloud_tgt_demean,
const Eigen::Vector4f ¢roid_tgt,
Eigen::Matrix4f &transformation_matrix)
{
transformation_matrix.setIdentity ();
// Assemble the correlation matrix H = source * target'
Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
// Compute the Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f u = svd.matrixU ();
Eigen::Matrix3f v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int x = 0; x < 3; ++x)
v (x, 2) *= -1;
}
Eigen::Matrix3f R = v * u.transpose ();
// Return the correct transformation
transformation_matrix.topLeftCorner<3, 3> () = R;
Eigen::Vector3f Rc = R * centroid_src.head<3> ();
transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
}
/* compute Jacobian */
void mmf::OptSO3ApproxGD::ComputeJacobian(const SO3f& thetaPrev, const
SO3f& theta, Eigen::Vector3f* J, float* f) {
Eigen::Matrix3f R = theta.matrix();
Eigen::Matrix3f Rprev = thetaPrev.matrix();
#ifndef NDEBUG
cout<<"qKarch"<<endl<<qKarch_<<endl;
cout<<"xSums_"<<endl<<xSums_<<endl;
cout<<"Ns_"<<endl<<Ns_<<endl;
#endif
if(J) for (uint32_t k=0; k<3; ++k)
(*J)(k) = tauR_*(Rprev.transpose()*SO3f::G(k)*R).trace();
if(f) *f = tauR_*(Rprev.transpose()*R).trace();
for (uint32_t j=0; j<6; ++j) {
if (Ns_(j) == 0) continue;
Eigen::Vector3f m = Eigen::Vector3f::Zero();
m(j/2) = j%2==0?-1.:1.;
float dot = max(-1.0f,min(1.0f,qKarch_.col(j).dot(R*m)));
float eps = acos(dot);
if (f) *f += Ns_(j)*eps*eps;
if (J) {
float factor = 0.;
if(-0.99< dot && dot < 0.99)
factor = -(2.*Ns_(j)*eps)/(sqrt(1.f-dot*dot));
else if(dot >= 0.99) {
// taylor series around 0.99 according to Mathematica
factor = -2.*Ns_(j)*(1.0033467240646519 - 0.33601724502488395
*(-0.99 + dot) + 0.13506297338381046* (-0.99 + dot)*(-0.99 + dot));
}else if (dot <= -0.99) {
factor = -2.*Ns_(j)*(21.266813135156017 - 1108.2484926534892*(0.99
+ dot) + 83235.29739487475*(0.99 + dot)*(0.99 + dot));
}
// std::cout << "factor " << factor << std::endl;
for (uint32_t k=0; k<3; ++k)
(*J)(k) += factor*qKarch_.col(j).dot(SO3f::G(k)*R*m);
}
}
if (f) *f /= -Ns_.sum();
if (J) *J /= -Ns_.sum();
}
void ARAPTerm::updateRotation()
{
Solver::DeformPetal& deform_petal = Solver::deform_petals_[petal_id_];
Solver::PetalMatrix& origin_petal = deform_petal._origin_petal;
Solver::PetalMatrix& petal_matrix = deform_petal._petal_matrix;
Solver::RotList& rot_list = deform_petal._R_list;
Solver::ScaleList& scale_list = deform_petal._S_list;
Solver::AdjList& adj_list = deform_petal._adj_list;
Solver::WeightMatrix& weight_matrix = deform_petal._weight_matrix;
Eigen::Matrix3f Si;
Eigen::MatrixXd Di;
Eigen::Matrix3Xd Pi_Prime;
Eigen::Matrix3Xd Pi;
for (size_t i = 0, i_end = rot_list.size(); i < i_end; ++i)
{
Di = Eigen::MatrixXd::Zero(adj_list[i].size(), adj_list[i].size());
Pi_Prime.resize(3, adj_list[i].size());
Pi.resize(3, adj_list[i].size());
for (size_t j = 0, j_end = adj_list[i].size(); j < j_end; ++j)
{
Di(j, j) = weight_matrix.coeffRef(i, adj_list[i][j]);
Pi.col(j) = origin_petal.col(i) - origin_petal.col(adj_list[i][j]);
Pi_Prime.col(j) = petal_matrix.col(i) - petal_matrix.col(adj_list[i][j]);
}
Si = Pi.cast<float>() * Di.cast<float>() * Pi_Prime.transpose().cast<float>();
Eigen::Matrix3f Ui;
Eigen::Vector3f Wi;
Eigen::Matrix3f Vi;
wunderSVD3x3(Si, Ui, Wi, Vi);
rot_list[i] = Vi.cast<double>() * Ui.transpose().cast<double>();
if (rot_list[i].determinant() < 0)
std::cout << "determinant is negative!" << std::endl;
double s = 0;
for (size_t j = 0, j_end = adj_list[i].size(); j < j_end; ++j)
{
s += Di(j, j) * Pi.col(j).squaredNorm();
}
// scale_list[i] = Wi.trace() / s;
/* if (scale_list[i] < 0.95 )
scale_list[i] = 0.95;
else if (scale_list[i] > 1.05)
scale_list[i] = 1.05;*/
}
/*if (petal_id_ == 0) std::cout << "vertex: " << 0 << " " << scale_list[0] << std::endl;*/
}
void CGLUtil::viewerGL()
{
glMatrixMode(GL_MODELVIEW);
// load the matrix to set camera pose
glLoadMatrixf(_eimModelViewGL.data());
//rotation
Eigen::Matrix3f eimRotation;
if( btl::utility::BTL_GL == _eConvention ){
eimRotation = Eigen::AngleAxisf(float(_dXAngle*M_PI/180.f), Eigen::Vector3f::UnitY())* Eigen::AngleAxisf(float(_dYAngle*M_PI/180.f), Eigen::Vector3f::UnitX()); // 3. rotate horizontally
}//mouse x-movement is the rotation around y-axis
else if( btl::utility::BTL_CV == _eConvention ) {
eimRotation = Eigen::AngleAxisf(float(_dXAngle*M_PI/180.f), -Eigen::Vector3f::UnitY())* Eigen::AngleAxisf(float(_dYAngle*M_PI/180.f), Eigen::Vector3f::UnitX()); // 3. rotate horizontally
}
//translation
/*_dZoom = _dZoom < 0.1? 0.1: _dZoom;
_dZoom = _dZoom > 10? 10: _dZoom;*/
//get direction N pointing from camera center to the object centroid
Eigen::Affine3f M; M = _eimModelViewGL;
Eigen::Vector3f T = M.translation();
Eigen::Matrix3f R = M.linear();
Eigen::Vector3f C = -R.transpose()*T;//camera centre
Eigen::Vector3f N = _eivCentroid - C;//from camera centre to object centroid
N = N/N.norm();//normalization
Eigen::Affine3f _eimManipulate; _eimManipulate.setIdentity();
_eimManipulate.translate( N*float(_dZoom) );//(N*(1-_dZoom)); //use camera movement toward object for zoom in/out effects
_eimManipulate.translate(_eivCentroid); // 5. translate back to the original camera pose
//_eimManipulate.scale(s); // 4. zoom in/out, never use scale to simulate camera movement, it affects z-buffer capturing. use translation instead
_eimManipulate.rotate(eimRotation); // 2. rotate vertically // 3. rotate horizontally
_eimManipulate.translate(-_eivCentroid); // 1. translate the camera center to align with object centroid*/
glMultMatrixf(_eimManipulate.data());
/*
lTranslated( _aCentroid[0], _aCentroid[1], _aCentroid[2] ); // 5. translate back to the original camera pose
_dZoom = _dZoom < 0.1? 0.1: _dZoom;
_dZoom = _dZoom > 10? 10: _dZoom;
glScaled( _dZoom, _dZoom, _dZoom ); // 4. zoom in/out,
if( btl::utility::BTL_GL == _eConvention )
glRotated ( _dXAngle, 0, 1 ,0 ); // 3. rotate horizontally
else if( btl::utility::BTL_CV == _eConvention ) //mouse x-movement is the rotation around y-axis
glRotated ( _dXAngle, 0,-1 ,0 );
glRotated ( _dYAngle, 1, 0 ,0 ); // 2. rotate vertically
glTranslated(-_aCentroid[0],-_aCentroid[1],-_aCentroid[2] ); // 1. translate the camera center to align with object centroid
*/
// light position in 3d
glLightfv(GL_LIGHT0, GL_POSITION, _aLight);
}
void CUDABuildLinearSystemRGBD::applyBL(CameraTrackingInput cameraTrackingInput, Eigen::Matrix3f& intrinsics, CameraTrackingParameters cameraTrackingParameters, float3 anglesOld, float3 translationOld, unsigned int imageWidth, unsigned int imageHeight, unsigned int level, Matrix6x7f& res, LinearSystemConfidence& conf)
{
unsigned int localWindowSize = 12;
if(level != 0) localWindowSize = max(1, localWindowSize/(4*level));
const unsigned int blockSize = 64;
const unsigned int dimX = (unsigned int)ceil(((float)imageWidth*imageHeight)/(localWindowSize*blockSize));
Eigen::Matrix3f intrinsicsRowMajor = intrinsics.transpose(); // Eigen is col major / cuda is row major
computeNormalEquations(imageWidth, imageHeight, d_output, cameraTrackingInput, intrinsicsRowMajor.data(), cameraTrackingParameters, anglesOld, translationOld, localWindowSize, blockSize);
cutilSafeCall(cudaMemcpy(h_output, d_output, sizeof(float)*30*dimX, cudaMemcpyDeviceToHost));
// Copy to CPU
res = reductionSystemCPU(h_output, dimX, conf);
}
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD (
const pcl::PointCloud<PointT> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointT> &cloud_tgt,
const std::vector<int> &indices_tgt,
Eigen::VectorXf &transform)
{
transform.resize (16);
Eigen::Vector4f centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (cloud_src, indices_src, centroid_src);
compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);
// Subtract the centroids from source, target
Eigen::MatrixXf cloud_src_demean;
demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
Eigen::MatrixXf cloud_tgt_demean;
demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);
// Assemble the correlation matrix H = source * target'
Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
// Compute the Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f u = svd.matrixU ();
Eigen::Matrix3f v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int x = 0; x < 3; ++x)
v (x, 2) *= -1;
}
Eigen::Matrix3f R = v * u.transpose ();
// Return the correct transformation
transform.segment<3> (0) = R.row (0); transform[12] = 0;
transform.segment<3> (4) = R.row (1); transform[13] = 0;
transform.segment<3> (8) = R.row (2); transform[14] = 0;
Eigen::Vector3f t = centroid_tgt.head<3> () - R * centroid_src.head<3> ();
transform[3] = t[0]; transform[7] = t[1]; transform[11] = t[2]; transform[15] = 1.0;
}
/**
* estimateRigidTransformationSVD
*/
void RigidTransformationRANSAC::estimateRigidTransformationSVD(
const std::vector<Eigen::Vector3f > &srcPts,
const std::vector<int> &srcIndices,
const std::vector<Eigen::Vector3f > &tgtPts,
const std::vector<int> &tgtIndices,
Eigen::Matrix4f &transform)
{
Eigen::Vector3f srcCentroid, tgtCentroid;
// compute the centroids of source, target
ComputeCentroid (srcPts, srcIndices, srcCentroid);
ComputeCentroid (tgtPts, tgtIndices, tgtCentroid);
// Subtract the centroids from source, target
Eigen::MatrixXf srcPtsDemean;
DemeanPoints(srcPts, srcIndices, srcCentroid, srcPtsDemean);
Eigen::MatrixXf tgtPtsDemean;
DemeanPoints(tgtPts, tgtIndices, tgtCentroid, tgtPtsDemean);
// Assemble the correlation matrix H = source * target'
Eigen::Matrix3f H = (srcPtsDemean * tgtPtsDemean.transpose ()).topLeftCorner<3, 3>();
// Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f u = svd.matrixU ();
Eigen::Matrix3f v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int x = 0; x < 3; ++x)
v (x, 2) *= -1;
}
// Return the transformation
Eigen::Matrix3f R = v * u.transpose ();
Eigen::Vector3f t = tgtCentroid - R * srcCentroid;
// set rotation
transform.block(0,0,3,3) = R;
// set translation
transform.block(0,3,3,1) = t;
transform.block(3, 0, 1, 3).setZero();
transform(3,3) = 1.;
}
// odometry:
// x: distance traveled in local x-direction
// y: distance traveled in local y-direction
// phi: rotation update
void ExtendedKalmanFilter::predictionStep(const Eigen::Vector3f& odometry)
{
state(0) = state(0) + cos(state(2))*odometry(0) - sin(state(2))*odometry(1);
state(1) = state(1) + sin(state(2))*odometry(0) + cos(state(2))*odometry(1);
state(2) = state(2) + odometry(2);
state(2) = atan2(sin(state(2)),cos(state(2))); // normalize angle
// dg/dx:
Eigen::Matrix3f G;
G << 1, 0, -sin(state(2))*odometry(0) - cos(state(2))*odometry(1),
0, 1, cos(state(2))*odometry(0) - sin(state(2))*odometry(1),
0, 0, 1;
sigma = G*sigma*G.transpose() + Q;
}
void
drawBoundingBox(PointCloudT::Ptr cloud,
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer,
int z)
{
//Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud, centroid);
//Eigen::Matrix3f covariance;
computeCovarianceMatrixNormalized(*cloud, centroid, covariance);
//Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance,
//Eigen::ComputeEigenvectors);
eigen_solver.compute(covariance,Eigen::ComputeEigenvectors);
// eigen_solver = boost::shared_ptr<Eigen::SelfAdjointEigenSolver>
// (covariance,Eigen::ComputeEigenvectors);
eigDx = eigen_solver.eigenvectors();
eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1));
//Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity());
p2w.block<3,3>(0,0) = eigDx.transpose();
p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>());
//pcl::PointCloud<PointT> cPoints;
pcl::transformPointCloud(*cloud, cPoints, p2w);
//PointT min_pt, max_pt;
pcl::getMinMax3D(cPoints, min_pt, max_pt);
const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());
const Eigen::Quaternionf qfinal(eigDx);
const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>();
//viewer->addPointCloud(cloud);
viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z,boost::lexical_cast<std::string>((z+1)*200));
}
void PinholePointProjector::unProject(PointVector &points,
Gaussian3fVector &gaussians,
IntImage &indexImage,
const DepthImage &depthImage) const {
assert(depthImage.rows > 0 && depthImage.cols > 0 && "PinholePointProjector: Depth image has zero dimensions");
points.resize(depthImage.rows * depthImage.cols);
gaussians.resize(depthImage.rows * depthImage.cols);
indexImage.create(depthImage.rows, depthImage.cols);
int count = 0;
Point *point = &points[0];
Gaussian3f *gaussian = &gaussians[0];
float fB = _baseline * _cameraMatrix(0, 0);
Eigen::Matrix3f J;
for(int r = 0; r < depthImage.rows; r++) {
const float *f = &depthImage(r, 0);
int *i = &indexImage(r, 0);
for(int c = 0; c < depthImage.cols; c++, f++, i++) {
if(!_unProject(*point, c, r, *f)) {
*i = -1;
continue;
}
float z = *f;
float zVariation = (_alpha * z * z) / (fB + z * _alpha);
J <<
z, 0, (float)r,
0, z, (float)c,
0, 0, 1;
J = _iK * J;
Diagonal3f imageCovariance(3.0f, 3.0f, zVariation);
Eigen::Matrix3f cov = J * imageCovariance * J.transpose();
*gaussian = Gaussian3f(point->head<3>(), cov);
gaussian++;
point++;
*i = count;
count++;
}
}
points.resize(count);
gaussians.resize(count);
}
bool Utils::
factorViewMatrix(const Eigen::Projective3f& iMatrix,
Eigen::Matrix3f& oCalib, Eigen::Isometry3f& oPose,
bool& oIsOrthographic) {
oIsOrthographic = isOrthographic(iMatrix.matrix());
// get appropriate rows
std::vector<int> rows = {0,1,2};
if (!oIsOrthographic) rows[2] = 3;
// get A matrix (upper left 3x3) and t vector
Eigen::Matrix3f A;
Eigen::Vector3f t;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
A(i,j) = iMatrix(rows[i],j);
}
t[i] = iMatrix(rows[i],3);
}
// determine translation vector
oPose.setIdentity();
oPose.translation() = -(A.inverse()*t);
// determine calibration matrix
Eigen::Matrix3f AAtrans = A*A.transpose();
AAtrans.col(0).swap(AAtrans.col(2));
AAtrans.row(0).swap(AAtrans.row(2));
Eigen::LLT<Eigen::Matrix3f, Eigen::Upper> llt(AAtrans);
oCalib = llt.matrixU();
oCalib.col(0).swap(oCalib.col(2));
oCalib.row(0).swap(oCalib.row(2));
oCalib.transposeInPlace();
// compute rotation matrix
oPose.linear() = (oCalib.inverse()*A).transpose();
return true;
}
///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
Eigen::Matrix3f
sasmath::Math::
find_u(sasmol::SasMol &mol, int &frame)
{
Eigen::Matrix3f r ;
float rxx = (mol._x().col(frame)*_x().col(frame)).sum() ;
float rxy = (mol._x().col(frame)*_y().col(frame)).sum() ;
float rxz = (mol._x().col(frame)*_z().col(frame)).sum() ;
float ryx = (mol._y().col(frame)*_x().col(frame)).sum() ;
float ryy = (mol._y().col(frame)*_y().col(frame)).sum() ;
float ryz = (mol._y().col(frame)*_z().col(frame)).sum() ;
float rzx = (mol._z().col(frame)*_x().col(frame)).sum() ;
float rzy = (mol._z().col(frame)*_y().col(frame)).sum() ;
float rzz = (mol._z().col(frame)*_z().col(frame)).sum() ;
r << rxx,rxy,rxz,
ryx,ryy,ryz,
rzx,rzy,rzz;
Eigen::Matrix3f rtr = r.transpose() * r ;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(rtr);
if (eigensolver.info() != Eigen::Success)
{
std::cout << "find_u calculation failed" << std::endl ;
}
Eigen::Matrix<float,3,1> uk ; // eigenvalues
Eigen::Matrix3f ak ; // eigenvectors
uk = eigensolver.eigenvalues() ;
ak = eigensolver.eigenvectors() ;
Eigen::Matrix3f akt = ak.transpose() ;
Eigen::Matrix3f new_ak ;
new_ak.row(0) = akt.row(2) ; //sort eigenvectors
new_ak.row(1) = akt.row(1) ;
new_ak.row(2) = akt.row(0) ;
// python ak0 --> ak[2] ; ak1 --> ak[1] ; ak2 --> ak[0]
//Eigen::Matrix<float,3,1> ak2 = akt.row(2).cross(akt.row(1)) ;
Eigen::MatrixXf rak0 = (r * (akt.row(2).transpose())) ;
Eigen::MatrixXf rak1 = (r * (akt.row(1).transpose())) ;
Eigen::Matrix<float,3,1> urak0 ;
if(uk[2] == 0.0)
{
urak0 = 1E15 * rak0 ;
}
else
{
urak0 = (1.0/sqrt(fabs(uk[2])))*rak0 ;
}
Eigen::Matrix<float,3,1> urak1 ;
if(uk[1] == 0.0)
{
urak1 = 1E15 * rak1 ;
}
else
{
urak1 = (1.0/sqrt(fabs(uk[1])))*rak1 ;
}
Eigen::Matrix<float,3,1> urak2 = urak0.col(0).cross(urak1.col(0)) ;
Eigen::Matrix3f bk ;
bk << urak0,urak1,urak2;
Eigen::Matrix3f u ;
u = (bk * new_ak).transpose() ;
return u ;
/*
u = array([[-0.94513198, 0.31068658, 0.100992 ],
[-0.3006246 , -0.70612572, -0.64110165],
[-0.12786863, -0.63628635, 0.76078203]])
check:
u =
-0.945133 0.310686 0.100992
-0.300624 -0.706126 -0.641102
-0.127869 -0.636287 0.760782
*/
}
Eigen::VectorXf solve_ik(Eigen::Matrix3f goalR, Eigen::Vector3f goalT,int sol_index)
{
cout << "[SOLVER] Solution " << sol_index << std::endl;
Eigen::VectorXf result(5);
//~ index:
//~ 0 direct space, low elbow
//~ 1 direct space, high elbow
//~ 2 inverse space, high elbow
//~ 3 inverse space, low elbow
Eigen::Matrix<float,5,4> DH;
DH.row(0) << 0.0, 0.019, 0.0330, M_PI/2;
DH.row(1) << 0.0, 0.0, 0.1550, 0.0;
DH.row(2) << 0.0, 0.0, 0.1350, 0.0;
DH.row(3) << 0.0, 0.0, 0.0, M_PI/2;
DH.row(4) << 0.0, 0.2175, 0.0, 0.0;
//~ from homing to DH structure
//~ offset = [deg2rad(169) deg2rad(65+90) -deg2rad(146) deg2rad(102-90) deg2rad(167)];
Eigen::VectorXf offset(5);
offset(0) = 169*DEG2RAD;
offset(1) = 155*DEG2RAD;
offset(2) = -146*DEG2RAD;
offset(3) = 192*DEG2RAD;
offset(4) = 167*DEG2RAD;
float L2 = 0.155;
float L3 = 0.135;
float L4 = 0.2175;
float beta = atan2(goalR(2,2), sqrt( pow(goalR(0,2),2) + pow(goalR(1,2),2) ) );
float q1, q2, q3, q4, q5, Z1, Z2, X1, X2;
float sin_theta3, cos_theta3, theta3, theta2;
if (sol_index == 0 || sol_index == 1)
{
//~ direct arm space
q1 = atan2(goalT(1),goalT(0));
Z1 = goalT(2) - DH(0,1);
Z2 = Z1 - L4*sin(beta);
X1 = sqrt( pow(goalT(0),2) + pow(goalT(1),2) ) - DH(0,2);
X2 = X1 - L4*cos(beta);
}
else
{
//~ inverse arm space
q1 = atan2(goalT(1),goalT(0)) - M_PI;
Z1 = goalT(2) - DH(0,1);
Z2 = Z1 - L4*sin(beta);
X1 = sqrt( pow(goalT(0),2) + pow(goalT(1),2) ) + DH(0,2);
X2 = X1 - L4*cos(beta);
}
cos_theta3 = (-pow(Z2,2) -pow(X2,2) + pow(L2,2) + pow(L3,2)) / (2*L2*L3);
if (cos_theta3 > -1 && cos_theta3 < 1)
{
// joint 3
if (sol_index == 0 || sol_index == 2)
{
sin_theta3 = sqrt(1-pow(cos_theta3,2));
theta3 = atan2(sin_theta3,cos_theta3);
q3 = M_PI - theta3;
}
else
{
sin_theta3 = -sqrt(1-pow(cos_theta3,2));
theta3 = atan2(sin_theta3,cos_theta3);
q3 = - M_PI - theta3;
}
// joint 2
float k1 = L2 + L3*cos(q3);
float k2 = L3*sin(q3);
if (sol_index == 0 || sol_index == 1)
{
theta2 = atan2(Z2,X2) - atan2(k2,k1);
q2 = theta2;
// joint 4
q4 = (M_PI/2) - (q2 + q3 - beta);
}
else
{
theta2 = atan2(Z2,X2) + atan2(k2,k1);
q2 = M_PI - theta2;
// joint 4
q4 = ( M_PI - (q2 + q3) - beta) + M_PI/2;
}
Eigen::MatrixXf T(4,4);
T = Eigen::MatrixXf::Identity(4,4);
Eigen::Vector4f joints(q1,q2,q3,q4);
float j, d, a, alpha;
for (int k=0; k<4; ++k)
{
j = joints(k);
d = DH(k,1);
a = DH(k,2);
alpha = DH(k,3);
// component transform matrix
Eigen::Matrix<float,4,4> CTM;
CTM.row(0) << cos(j), -sin(j)*cos(alpha), sin(j)*sin(alpha), a*cos(j);
CTM.row(1) << sin(j), cos(j)*cos(alpha), -cos(j)*sin(alpha), a*sin(j);
CTM.row(2) << 0.0, sin(alpha), cos(alpha), d;
CTM.row(3) << 0.0, 0.0, 0.0, 1;
T = T * CTM;
}
Eigen::Matrix3f tool = T.block(0,0,3,3);
Eigen::Matrix3f roll = tool.transpose() * goalR;
q5 = atan2(roll(1,0),roll(0,0));
T = Eigen::MatrixXf::Identity(4,4);
//~ The setpoint angle for joint arm_joint_1 is out of range. The valid range is between 0.0100692 rad and 5.84014 rad
//~ The setpoint angle for joint arm_joint_2 is out of range. The valid range is between 0.0100692 rad and 2.61799 rad
//~ The setpoint angle for joint arm_joint_3 is out of range. The valid range is between -5.02655 rad and -0.015708 rad
//~ The setpoint angle for joint arm_joint_4 is out of range. The valid range is between 0.0221239 rad and 3.4292 rad
//~ The setpoint angle for joint arm_joint_5 is out of range. The valid range is between 0.110619 rad and 5.64159 rad
result(0) = offset(0)-q1;
result(1) = offset(1)-q2;
result(2) = offset(2)-q3;
result(3) = offset(3)-q4;
result(4) = offset(4)-q5;
if (result(4) < 0.110619 )
{
result(4) = result(4) + M_PI;
}
else if ( result(4) > 5.64159)
{
result(4) = result(4) - M_PI;
}
}
else
{
// position not reachable: arm will stand still (we will give impossible values for the default controller)
result = result.setConstant(-10);
}
Eigen::Matrix<float,5,2> range;
range.row(0) << 0.0100692+0.0017, 5.84014-0.0017;
range.row(1) << 0.0100692+0.0017, 2.61799-0.0017;
range.row(2) << -5.02655+0.0017, -0.015708-0.0017;
range.row(3) << 0.0221239+0.0017, 3.4292-0.0017;
range.row(4) << 0.110619+0.0017, 5.64159-0.0017;
for(int k=0; k<5; ++k)
{
if( result(k) == -10)
{
cout << "[SOLVER] Solution " << sol_index << " discarded: desired goal not reachable by the robot" << std::endl;
result = result.setZero();
k = 5;
}
else if( range(k,0) > result(k) || range(k,1) < result(k) )
{
cout << "[SOLVER] Solution " << sol_index << " discarded: joint " << k+1 << " outside accetable ranges. Value: " << result(k) << "; Range: [" << range(k,0) << " , " << range(k,1) << "]" << std::endl;
result = result.setZero();
k = 5;
}
else
{
}
}
cout << "----------" << std::endl;
return result;
}
void TwoDepthImageAlignerNode::processNode(MapNode* node_){
cerr << "START ITERATION" << endl;
std::vector<Serializable*> crearedObjects;
SensingFrameNode* sensingFrame = dynamic_cast<SensingFrameNode*>(node_);
if (! sensingFrame)
return;
PinholeImageData* image = dynamic_cast<PinholeImageData*>(sensingFrame->sensorData(_topic));
if (! image)
return;
cerr << "got image" << endl;
Eigen::Isometry3d _sensorOffset = _config->sensorOffset(image->baseSensor());
// cerr << "sensorOffset: " << endl;
// cerr << _sensorOffset.matrix() << endl;
Eigen::Isometry3f sensorOffset;
convertScalar(sensorOffset,_sensorOffset);
sensorOffset.matrix().row(3) << 0,0,0,1;
Eigen::Matrix3d _cameraMatrix = image->cameraMatrix();
ImageBLOB* blob = image->imageBlob().get();
DepthImage depthImage;
depthImage.fromCvMat(blob->cvImage());
int r=depthImage.rows();
int c=depthImage.cols();
DepthImage scaledImage;
Eigen::Matrix3f cameraMatrix;
convertScalar(cameraMatrix,_cameraMatrix);
//computeScaledParameters(r,c,cameraMatrix,_scale);
PinholePointProjector* projector=dynamic_cast<PinholePointProjector*>(_converter->projector());
cameraMatrix(2,2)=1;
projector->setCameraMatrix(cameraMatrix);
projector->setImageSize(depthImage.rows(), depthImage.cols());
projector->scale(1.0/_scale);
DepthImage::scale(scaledImage,depthImage,_scale);
pwn::Frame* frame = new pwn::Frame;
_converter->compute(*frame,scaledImage, sensorOffset);
MapNodeBinaryRelation* odom=0;
std::vector<MapNode*> oneNode(1);
oneNode[0]=sensingFrame;
MapNodeUnaryRelation* imu = extractRelation<MapNodeUnaryRelation>(oneNode);
if (_previousFrame){
_aligner->setReferenceSensorOffset(_aligner->currentSensorOffset());
_aligner->setCurrentSensorOffset(sensorOffset);
_aligner->setReferenceFrame(_previousFrame);
_aligner->setCurrentFrame(frame);
PinholePointProjector* projector=(PinholePointProjector*)(_aligner->projector());
projector->setCameraMatrix(cameraMatrix);
projector->setImageSize(depthImage.rows(), depthImage.cols());
projector->scale(1.0/_scale);
_aligner->correspondenceFinder()->setImageSize(projector->imageRows(), projector->imageCols());
/*
cerr << "correspondenceFinder: " << r << " " << c << endl;
cerr << "sensorOffset" << endl;
cerr <<_aligner->currentSensorOffset().matrix() << endl;
cerr <<_aligner->referenceSensorOffset().matrix() << endl;
cerr << "cameraMatrix" << endl;
cerr << projector->cameraMatrix() << endl;
*/
std::vector<MapNode*> twoNodes(2);
twoNodes[0]=_previousSensingFrameNode;
twoNodes[1]=sensingFrame;
odom = extractRelation<MapNodeBinaryRelation>(twoNodes);
cerr << "odom:" << odom << " imu:" << imu << endl;
Eigen::Isometry3f guess= Eigen::Isometry3f::Identity();
_aligner->clearPriors();
if (odom){
Eigen::Isometry3f mean;
Eigen::Matrix<float,6,6> info;
convertScalar(mean,odom->transform());
mean.matrix().row(3) << 0,0,0,1;
convertScalar(info,odom->informationMatrix());
//cerr << "odom: " << t2v(mean).transpose() << endl;
_aligner->addRelativePrior(mean,info);
//guess = mean;
}
if (imu){
Eigen::Isometry3f mean;
Eigen::Matrix<float,6,6> info;
convertScalar(mean,imu->transform());
convertScalar(info,imu->informationMatrix());
mean.matrix().row(3) << 0,0,0,1;
//cerr << "imu: " << t2v(mean).transpose() << endl;
_aligner->addAbsolutePrior(_globalT,mean,info);
}
_aligner->setInitialGuess(guess);
cerr << "Frames: " << _previousFrame << " " << frame << endl;
// projector->setCameraMatrix(cameraMatrix);
// projector->setTransform(Eigen::Isometry3f::Identity());
// Eigen::MatrixXi debugIndices(r,c);
// DepthImage debugImage(r,c);
// projector->project(debugIndices, debugImage, frame->points());
_aligner->align();
// sprintf(buf, "img-dbg-%05d.pgm",j);
// debugImage.save(buf);
//sprintf(buf, "img-ref-%05d.pgm",j);
//_aligner->correspondenceFinder()->referenceDepthImage().save(buf);
//sprintf(buf, "img-cur-%05d.pgm",j);
//_aligner->correspondenceFinder()->currentDepthImage().save(buf);
cerr << "inliers: " << _aligner->inliers() << endl;
cerr << "chi2: " << _aligner->error() << endl;
cerr << "chi2/inliers: " << _aligner->error()/_aligner->inliers() << endl;
cerr << "initialGuess: " << t2v(guess).transpose() << endl;
cerr << "transform : " << t2v(_aligner->T()).transpose() << endl;
if (_aligner->inliers()>-1){
_globalT = _globalT*_aligner->T();
cerr << "TRANSFORM FOUND" << endl;
} else {
cerr << "FAILURE" << endl;
_globalT = _globalT*guess;
}
if (! (_counter%50) ) {
Eigen::Matrix3f R = _globalT.linear();
Eigen::Matrix3f E = R.transpose() * R;
E.diagonal().array() -= 1;
_globalT.linear() -= 0.5 * R * E;
}
_globalT.matrix().row(3) << 0,0,0,1;
cerr << "globalTransform : " << t2v(_globalT).transpose() << endl;
// char buf[1024];
// sprintf(buf, "frame-%05d.pwn",_counter);
// frame->save(buf, 1, true, _globalT);
cerr << "creating alias" << endl;
// create an alias for the previous node
MapNodeAlias* newRoot = new MapNodeAlias(_previousSensingFrameNode,_previousSensingFrameNode->manager());
_previousSensingFrameNode->manager()->addNode(newRoot);
cerr << "creating alias relation" << endl;
MapNodeAliasRelation* aliasRel = new MapNodeAliasRelation(newRoot,_previousSensingFrameNode->manager());
aliasRel->nodes()[0] = newRoot;
aliasRel->nodes()[1] = _previousSensingFrameNode;
_previousSensingFrameNode->manager()->addRelation(aliasRel);
cerr << "reparenting old elements" << endl;
// assign all the used relations to the alias node
if (imu){
imu->setOwner(newRoot);
imu->manager()->addRelation(imu);
}
if (odom){
odom->setOwner(newRoot);
odom->manager()->addRelation(imu);
}
cerr << "adding result" << endl;
Relation* newRel = new Relation(_aligner, _converter,
_previousSensingFrameNode, sensingFrame,
_topic, _aligner->inliers(), _aligner->error(), _manager);
newRel->setOwner(newRoot);
newRel->nodes()[0] = newRoot;
newRel->nodes()[1] = _previousSensingFrameNode;
Eigen::Isometry3d iso;
convertScalar(iso,_aligner->T());
newRel->setTransform(iso);
newRel->setInformationMatrix(Eigen::Matrix<double, 6,6>::Identity());
newRel->manager()->addRelation(newRel);
*os << _globalT.translation().transpose() << endl;
// create a new alias node for the prior element
// bind the alias with the prior node
// add to the alias the relations that have been used to
// determine the transformation
// add the alias to the map
// add the new transformation to the map
} else {
_aligner->setCurrentSensorOffset(sensorOffset);
_globalT = Eigen::Isometry3f::Identity();
if (imu){
Eigen::Isometry3f mean;
convertScalar(mean,imu->transform());
_globalT = mean;
}
}
cerr << "deleting previous frame" << endl;
if (_previousFrame)
delete _previousFrame;
cerr << "deleting blob frame" << endl;
delete blob;
cerr << "bookkeeping update" << endl;
_previousSensingFrameNode = sensingFrame;
_previousFrame = frame;
_counter++;
cerr << "END ITERATION" << endl;
}
void
LocalRecognitionPipeline<PointT>::correspondenceGrouping ()
{
if(cg_algorithm_->getRequiresNormals() && (!scene_normals_ || scene_normals_->points.size() != scene_->points.size()))
computeNormals<PointT>(scene_, scene_normals_, param_.normal_computation_method_);
typename std::map<std::string, ObjectHypothesis<PointT> >::iterator it;
for (it = obj_hypotheses_.begin (); it != obj_hypotheses_.end (); it++)
{
ObjectHypothesis<PointT> &oh = it->second;
if(oh.model_scene_corresp_.size() < 3)
continue;
std::vector < pcl::Correspondences > corresp_clusters;
cg_algorithm_->setSceneCloud (scene_);
cg_algorithm_->setInputCloud (oh.model_->keypoints_);
if(cg_algorithm_->getRequiresNormals())
cg_algorithm_->setInputAndSceneNormals(oh.model_->kp_normals_, scene_normals_);
//we need to pass the keypoints_pointcloud and the specific object hypothesis
cg_algorithm_->setModelSceneCorrespondences (oh.model_scene_corresp_);
cg_algorithm_->cluster (corresp_clusters);
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > new_transforms (corresp_clusters.size());
typename pcl::registration::TransformationEstimationSVD < PointT, PointT > t_est;
for (size_t i = 0; i < corresp_clusters.size(); i++)
t_est.estimateRigidTransformation (*oh.model_->keypoints_, *scene_, corresp_clusters[i], new_transforms[i]);
if(param_.merge_close_hypotheses_) {
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > merged_transforms (corresp_clusters.size());
std::vector<bool> cluster_has_been_taken(corresp_clusters.size(), false);
const double angle_thresh_rad = param_.merge_close_hypotheses_angle_ * M_PI / 180.f ;
size_t kept=0;
for (size_t i = 0; i < new_transforms.size(); i++) {
if (cluster_has_been_taken[i])
continue;
cluster_has_been_taken[i] = true;
const Eigen::Vector3f centroid1 = new_transforms[i].block<3, 1> (0, 3);
const Eigen::Matrix3f rot1 = new_transforms[i].block<3, 3> (0, 0);
pcl::Correspondences merged_corrs = corresp_clusters[i];
for(size_t j=i; j < new_transforms.size(); j++) {
const Eigen::Vector3f centroid2 = new_transforms[j].block<3, 1> (0, 3);
const Eigen::Matrix3f rot2 = new_transforms[j].block<3, 3> (0, 0);
const Eigen::Matrix3f rot_diff = rot2 * rot1.transpose();
double rotx = atan2(rot_diff(2,1), rot_diff(2,2));
double roty = atan2(-rot_diff(2,0), sqrt(rot_diff(2,1) * rot_diff(2,1) + rot_diff(2,2) * rot_diff(2,2)));
double rotz = atan2(rot_diff(1,0), rot_diff(0,0));
double dist = (centroid1 - centroid2).norm();
if ( (dist < param_.merge_close_hypotheses_dist_) && (rotx < angle_thresh_rad) && (roty < angle_thresh_rad) && (rotz < angle_thresh_rad) ) {
merged_corrs.insert( merged_corrs.end(), corresp_clusters[j].begin(), corresp_clusters[j].end() );
cluster_has_been_taken[j] = true;
}
}
t_est.estimateRigidTransformation (*oh.model_->keypoints_, *scene_, merged_corrs, merged_transforms[kept]);
kept++;
}
merged_transforms.resize(kept);
new_transforms = merged_transforms;
}
std::cout << "Merged " << corresp_clusters.size() << " clusters into " << new_transforms.size() << " clusters. Total correspondences: " << oh.model_scene_corresp_.size () << " " << it->first << std::endl;
// oh.visualize(*scene_);
size_t existing_hypotheses = models_.size();
models_.resize( existing_hypotheses + new_transforms.size(), oh.model_ );
transforms_.insert(transforms_.end(), new_transforms.begin(), new_transforms.end());
}
}
template<template<class > class Distance, typename PointT, typename FeatureT> bool
GlobalNNPipelineROS<Distance,PointT,FeatureT>::classifyROS (classifier_srv_definitions::classify::Request & req, classifier_srv_definitions::classify::Response & response)
{
typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>());
pcl::fromROSMsg(req.cloud, *cloud);
this->input_ = cloud;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pClusteredPCl (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::copyPointCloud(*cloud, *pClusteredPCl);
//clear all data from previous visualization steps and publish empty markers/point cloud
for (size_t i=0; i < markerArray_.markers.size(); i++)
{
markerArray_.markers[i].action = visualization_msgs::Marker::DELETE;
}
vis_pub_.publish( markerArray_ );
sensor_msgs::PointCloud2 scenePc2;
vis_pc_pub_.publish(scenePc2);
markerArray_.markers.clear();
for(size_t cluster_id=0; cluster_id < req.clusters_indices.size(); cluster_id++)
{
std::vector<int> cluster_indices_int;
Eigen::Vector4f centroid;
Eigen::Matrix3f covariance_matrix;
const float r = std::rand() % 255;
const float g = std::rand() % 255;
const float b = std::rand() % 255;
this->indices_.resize(req.clusters_indices[cluster_id].data.size());
for(size_t kk=0; kk < req.clusters_indices[cluster_id].data.size(); kk++)
{
this->indices_[kk] = static_cast<int>(req.clusters_indices[cluster_id].data[kk]);
pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).r = 0.8*r;
pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).g = 0.8*g;
pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).b = 0.8*b;
}
this->classify ();
std::cout << "for cluster " << cluster_id << " with size " << cluster_indices_int.size() << ", I have following hypotheses: " << std::endl;
object_perception_msgs::classification class_tmp;
for(size_t kk=0; kk < this->categories_.size(); kk++)
{
std::cout << this->categories_[kk] << " with confidence " << this->confidences_[kk] << std::endl;
std_msgs::String str_tmp;
str_tmp.data = this->categories_[kk];
class_tmp.class_type.push_back(str_tmp);
class_tmp.confidence.push_back( this->confidences_[kk] );
}
response.class_results.push_back(class_tmp);
typename pcl::PointCloud<PointT>::Ptr pClusterPCl_transformed (new pcl::PointCloud<PointT>());
pcl::computeMeanAndCovarianceMatrix(*this->input_, cluster_indices_int, covariance_matrix, centroid);
Eigen::Matrix3f eigvects;
Eigen::Vector3f eigvals;
pcl::eigen33(covariance_matrix, eigvects, eigvals);
Eigen::Vector3f centroid_transformed = eigvects.transpose() * centroid.topRows(3);
Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Zero(4,4);
transformation_matrix.block<3,3>(0,0) = eigvects.transpose();
transformation_matrix.block<3,1>(0,3) = -centroid_transformed;
transformation_matrix(3,3) = 1;
pcl::transformPointCloud(*this->input_, cluster_indices_int, *pClusterPCl_transformed, transformation_matrix);
//pcl::transformPointCloud(*frame_, cluster_indices_int, *frame_eigencoordinates_, eigvects);
PointT min_pt, max_pt;
pcl::getMinMax3D(*pClusterPCl_transformed, min_pt, max_pt);
std::cout << "Elongations along eigenvectors: " << max_pt.x - min_pt.x << ", " << max_pt.y - min_pt.y
<< ", " << max_pt.z - min_pt.z << std::endl;
geometry_msgs::Point32 centroid_ros;
centroid_ros.x = centroid[0];
centroid_ros.y = centroid[1];
centroid_ros.z = centroid[2];
response.centroid.push_back(centroid_ros);
// calculating the bounding box of the cluster
Eigen::Vector4f min;
Eigen::Vector4f max;
pcl::getMinMax3D (*this->input_, cluster_indices_int, min, max);
object_perception_msgs::BBox bbox;
geometry_msgs::Point32 pt;
pt.x = min[0]; pt.y = min[1]; pt.z = min[2]; bbox.point.push_back(pt);
pt.x = min[0]; pt.y = min[1]; pt.z = max[2]; bbox.point.push_back(pt);
pt.x = min[0]; pt.y = max[1]; pt.z = min[2]; bbox.point.push_back(pt);
pt.x = min[0]; pt.y = max[1]; pt.z = max[2]; bbox.point.push_back(pt);
pt.x = max[0]; pt.y = min[1]; pt.z = min[2]; bbox.point.push_back(pt);
pt.x = max[0]; pt.y = min[1]; pt.z = max[2]; bbox.point.push_back(pt);
pt.x = max[0]; pt.y = max[1]; pt.z = min[2]; bbox.point.push_back(pt);
pt.x = max[0]; pt.y = max[1]; pt.z = max[2]; bbox.point.push_back(pt);
response.bbox.push_back(bbox);
// getting the point cloud of the cluster
typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
pcl::copyPointCloud(*this->input_, cluster_indices_int, *cluster);
sensor_msgs::PointCloud2 pc2;
pcl::toROSMsg (*cluster, pc2);
response.cloud.push_back(pc2);
// visualize the result as ROS topic
visualization_msgs::Marker marker;
marker.header.frame_id = camera_frame_;
marker.header.stamp = ros::Time::now();
//marker.header.seq = ++marker_seq_;
marker.ns = "object_classification";
marker.id = cluster_id;
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = centroid_ros.x;
marker.pose.position.y = centroid_ros.y - 0.1;
marker.pose.position.z = centroid_ros.z - 0.1;
marker.pose.orientation.x = 0;
marker.pose.orientation.y = 0;
marker.pose.orientation.z = 0;
marker.pose.orientation.w = 1.0;
marker.scale.z = 0.1;
marker.color.a = 1.0;
marker.color.r = r/255.f;
marker.color.g = g/255.f;
marker.color.b = b/255.f;
std::stringstream marker_text;
marker_text.precision(2);
marker_text << this->categories_[0] << this->confidences_[0];
marker.text = marker_text.str();
markerArray_.markers.push_back(marker);
}
pcl::toROSMsg (*pClusteredPCl, scenePc2);
vis_pc_pub_.publish(scenePc2);
vis_pub_.publish( markerArray_ );
response.clusters_indices = req.clusters_indices;
return true;
}
void
NMBasedCloudIntegration<PointT>::collectInfo ()
{
size_t total_point_count = 0;
for(size_t i = 0; i < input_clouds_.size(); i++)
total_point_count += (indices_.empty() || indices_[i].empty()) ? input_clouds_[i]->size() : indices_[i].size();
VLOG(1) << "Allocating memory for point information of " << total_point_count << "points. ";
big_cloud_info_.resize(total_point_count);
std::vector<pcl::PointCloud<PointT> > input_clouds_aligned (input_clouds_.size());
std::vector<pcl::PointCloud<pcl::Normal> > input_normals_aligned (input_clouds_.size());
#pragma omp parallel for schedule(dynamic)
for(size_t i=0; i < input_clouds_.size(); i++)
{
pcl::transformPointCloud(*input_clouds_[i], input_clouds_aligned[i], transformations_to_global_[i]);
transformNormals(*input_normals_[i], input_normals_aligned[i], transformations_to_global_[i]);
}
size_t point_count = 0;
for(size_t i=0; i < input_clouds_.size(); i++)
{
const pcl::PointCloud<PointT> &cloud_aligned = input_clouds_aligned[i];
const pcl::PointCloud<pcl::Normal> &normals_aligned = input_normals_aligned[i];
size_t kept_new_pts = 0;
if (indices_.empty() || indices_[i].empty())
{
for(size_t jj=0; jj<cloud_aligned.points.size(); jj++)
{
if ( !pcl::isFinite(cloud_aligned.points[jj]) || !pcl::isFinite(normals_aligned.points[jj]) )
continue;
PointInfo &pt = big_cloud_info_[point_count + kept_new_pts];
pt.pt = cloud_aligned.points[jj];
pt.normal = normals_aligned.points[jj];
pt.sigma_lateral = pt_properties_[i][jj][0];
pt.sigma_axial = pt_properties_[i][jj][1];
pt.distance_to_depth_discontinuity = pt_properties_[i][jj][2];
pt.pt_idx = jj;
kept_new_pts++;
}
}
else
{
for(int idx : indices_[i])
{
if ( !pcl::isFinite(cloud_aligned.points[idx]) || !pcl::isFinite(normals_aligned.points[idx]) )
continue;
PointInfo &pt = big_cloud_info_[point_count + kept_new_pts];
pt.pt = cloud_aligned.points[idx];
pt.normal = normals_aligned.points[ idx ];
pt.sigma_lateral = pt_properties_[i][idx][0];
pt.sigma_axial = pt_properties_[i][idx][1];
pt.distance_to_depth_discontinuity = pt_properties_[i][idx][2];
pt.pt_idx = idx;
kept_new_pts++;
}
}
// compute and store remaining information
#pragma omp parallel for schedule (dynamic) firstprivate(i, point_count, kept_new_pts)
for(size_t jj=0; jj<kept_new_pts; jj++)
{
PointInfo &pt = big_cloud_info_ [point_count + jj];
pt.origin = i;
Eigen::Matrix3f sigma = Eigen::Matrix3f::Zero(), sigma_aligned = Eigen::Matrix3f::Zero();
sigma(0,0) = pt.sigma_lateral;
sigma(1,1) = pt.sigma_lateral;
sigma(2,2) = pt.sigma_axial;
const Eigen::Matrix4f &tf = transformations_to_global_[ i ];
Eigen::Matrix3f rotation = tf.block<3,3>(0,0); // or inverse?
sigma_aligned = rotation * sigma * rotation.transpose();
double det = sigma_aligned.determinant();
// if( std::isfinite(det) && det>0)
// pt.probability = 1 / sqrt(2 * M_PI * det);
// else
// pt.probability = std::numeric_limits<float>::min();
if( std::isfinite(det) && det>0)
pt.weight = det;
else
pt.weight = std::numeric_limits<float>::max();
}
point_count += kept_new_pts;
}
big_cloud_info_.resize(point_count);
}
int
main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("../learn15.pcd", *cloud) == -1) //* load the file
{
PCL_ERROR("Couldn't read file test_pcd.pcd \n");
return (-1);
}
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
PCL_ERROR("Could not estimate a planar model for the given dataset.");
return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(true);
//Removes part_of_cloud but retain the original full_cloud
//extract.setNegative(true); // Removes part_of_cloud from full cloud and keep the rest
extract.filter(*cloud_plane);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud(cloud_plane);
sor.setMeanK(50);
sor.setStddevMulThresh(1.0);
sor.filter(*cloud_filtered);
//cloud.swap(cloud_plane);
/*
pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
viewer.showCloud(cloud_plane);
while (!viewer.wasStopped())
{
}
*/
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
Eigen::Matrix<float, 1, 3> fitted_plane_norm, xyaxis_plane_norm, rotation_axis;
fitted_plane_norm[0] = coefficients->values[0];
fitted_plane_norm[1] = coefficients->values[1];
fitted_plane_norm[2] = coefficients->values[2];
xyaxis_plane_norm[0] = 0.0;
xyaxis_plane_norm[1] = 0.0;
xyaxis_plane_norm[2] = 1.0;
rotation_axis = xyaxis_plane_norm.cross(fitted_plane_norm);
float theta = acos(xyaxis_plane_norm.dot(fitted_plane_norm));
//float theta = -atan2(rotation_axis.norm(), xyaxis_plane_norm.dot(fitted_plane_norm));
transform_2.rotate(Eigen::AngleAxisf(theta, rotation_axis));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_recentered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::transformPointCloud(*cloud_filtered, *cloud_recentered, transform_2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud_recentered, *cloud_xyz);
///////////////////////////////////////////////////////////////////
Eigen::Vector4f pcaCentroid;
pcl::compute3DCentroid(*cloud_xyz, pcaCentroid);
Eigen::Matrix3f covariance;
computeCovarianceMatrixNormalized(*cloud_xyz, pcaCentroid, covariance);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
std::cout << eigenVectorsPCA.size() << std::endl;
if(eigenVectorsPCA.size()!=9)
eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity());
projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();
projectionTransform.block<3, 1>(0, 3) = -1.f * (projectionTransform.block<3, 3>(0, 0) * pcaCentroid.head<3>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointsProjected(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud_xyz, *cloudPointsProjected, projectionTransform);
// Get the minimum and maximum points of the transformed cloud.
pcl::PointXYZ minPoint, maxPoint;
pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);
const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap());
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = rgbVis(cloud);
// viewer->addPointCloud<pcl::PointXYZ>(cloudPointsProjected, "Simple Cloud");
// viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "Simple Cloud2");
viewer->addCube(minPoint.x, maxPoint.x, minPoint.y, maxPoint.y, minPoint.z , maxPoint.z);
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
/*
pcl::PCA< pcl::PointXYZ > pca;
pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud;
pcl::PointCloud< pcl::PointXYZ > proj;
pca.setInputCloud(cloud);
pca.project(*cloud, proj);
Eigen::Vector4f proj_min;
Eigen::Vector4f proj_max;
pcl::getMinMax3D(proj, proj_min, proj_max);
pcl::PointXYZ min;
pcl::PointXYZ max;
pca.reconstruct(proj_min, min);
pca.reconstruct(proj_max, max);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer->addCube(min.x, max.x, min.y, max.y, min.z, max.z);
*/
return (0);
}
int main(int argc, char** argv)
{
// Punktwolke laden
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_unfiltered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile <pcl::PointXYZ> ("Arena_24_7_2014.pcd",*cloud_unfiltered) == -1)
{
std::cout<<"Cloud reading failed"<<std::endl;
return (-1);
}
pcl::PCDWriter writer;
//Filter für Höhen- und Tiefenbegrenzung:
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud_unfiltered);
pass.setFilterFieldName("z");
pass.setFilterLimits(-0.2,0.3);//Für Stativ ca. (-0.9,-0.2)
pass.filter(*cloud);
// Berechnen der Punktnormalen
pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod(tree);
normal_estimator.setInputCloud(cloud);
normal_estimator.setKSearch(100);// Ursprünglich:50
// http://pointclouds.org/documentation/tutorials/normal_estimation.php
normal_estimator.compute(*normals);
// Segmentierung mit dem RegionGrowing-Algorithmus (Setzen der Parameter)
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
reg.setMinClusterSize (200);
//reg.setMaxClusterSize (10000);
reg.setSearchMethod (tree);
reg.setNumberOfNeighbours (50); // Ursprünglich 50 (Bei ca. 150 werden die Kanten schön glatt!)
// aber es wird nach zu vielen Nachbarn gecheckt-> Mehrere Banden fallen in ein Cluster
reg.setInputCloud (cloud);
//reg.setIndices (indices);
reg.setInputNormals (normals);
reg.setSmoothnessThreshold (4.0 / 180.0 * M_PI); // Ursprünglich: 7.0/180*M_PI
// je kleiner desto weniger punkte sind "smooth" genug ->Klares Abgrenzen der Cluster!
reg.setCurvatureThreshold (.3);//Ursprünglich:1.0 // je kleiner desto geringer darf sich das Cluster krümmen
// Anwendung des Cluster-Filters auf Input-Wolke "cloud"
std::vector <pcl::PointIndices> clusters;
reg.extract (clusters);
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr planes_cloud (new pcl::PointCloud<pcl::PointXYZ>);
//vtkSmartPointer<vtkAppendPolyData> appendFilter =
//vtkSmartPointer<vtkAppendPolyData>::New();
//vtkSmartPointer<vtkPolyData> polyplane =
//vtkSmartPointer<vtkPolyData>::New();
int b=0;
// Schleife über alle detektierten Cluster
for(int a=0;a<clusters.size();a++)
{
if(clusters[a].indices.size() >100 )
{
pcl::IndicesPtr indices_ptr (new std::vector<int> (clusters[a].indices.size ()));
for (int i=0;i<indices_ptr->size();i++)
{
(*indices_ptr)[i]=clusters[a].indices[i]; // http://www.pcl-users.org/Removing-a-cluster-Problem-with-pointer-td4023699.html
} // Indizes des jeweiligen Clusters werden alle in indices_ptr gespeichert
// Punkte des Clusters werden in cluster_cloud geschrieben
extract.setIndices(indices_ptr);
extract.setNegative(false);
extract.filter(*cluster_cloud);// Punkte des Cluster a werden in cluster_cloud geschrieben
std::cout<<"cluster_cloud["<<a<<"] hat "<<cluster_cloud->width*cluster_cloud->height<<" Punkte."<<std::endl;
//Erzeugen einer Ebene aus Cluster_cloud
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(0.1);
seg.setInputCloud(cluster_cloud);
seg.segment(*inliers, *coefficients);
// Wenn Ebene vertikal: Abspeichern in Cluster_i.pcd
if(coefficients->values[2]<.9 && coefficients->values[2]>(-.9))//ax+by+cz+d=0 (wenn c=0 => Ebene parallel zur z-Achse)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr planes_projected (new pcl::PointCloud<pcl::PointXYZ>);
//Inliers auf Ebene projizieren:
pcl::PCA<pcl::PointXYZ> pca2;
pcl::ProjectInliers<pcl::PointXYZ> proj2;
proj2.setModelType(pcl::SACMODEL_PLANE);
proj2.setIndices(inliers);
proj2.setInputCloud(cluster_cloud);
proj2.setModelCoefficients(coefficients);
proj2.filter(*planes_projected);
// Punkte in Eigenraum transformieren, um bounding box zu berechnen (Quelle: pcl-users Forum (http://www.pcl-users.org/Finding-oriented-bounding-box-of-a-cloud-td4024616.html)
// compute principal direction
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*planes_projected, centroid);
Eigen::Matrix3f covariance;
computeCovarianceMatrixNormalized(*planes_projected, centroid, covariance);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
Eigen::Matrix3f eigDx = eigen_solver.eigenvectors();
eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1));
// move the points to the that reference frame
Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity());
p2w.block<3,3>(0,0) = eigDx.transpose();
p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>());
pcl::PointCloud<pcl::PointXYZ> cPoints;
pcl::transformPointCloud(*planes_projected, cPoints, p2w);
pcl::PointXYZ min_pt, max_pt;
pcl::getMinMax3D(cPoints, min_pt, max_pt);
const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());
// final transform
const Eigen::Quaternionf qfinal(eigDx);
const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>();
// Punktwolke und bounding box im viewer anzeigen
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer);
viewer->addCoordinateSystem ();
viewer->addPointCloud(planes_projected);// Danach auskommentieren
viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z);
// Ausgabe der Eckpunkte (kann gelöscht werden)
std::cout << " min.x= " << min_pt.x << " max.x= " << max_pt.x << " min.y= " << min_pt.y << " max.y= " << max_pt.y << " min.z= " << min_pt.z << " max.z= " << max_pt.z<< std::endl;
std::cout << "Punkte: " << min_pt.x <<";" << min_pt.y << ";" << min_pt.z <<std::endl;
std::cout << min_pt.x <<";" << min_pt.y << ";" << max_pt.z <<std::endl;
std::cout << min_pt.x <<";" << max_pt.y << ";" << min_pt.z <<std::endl;
std::cout << min_pt.x <<";" << max_pt.y << ";" << max_pt.z <<std::endl;
std::cout << max_pt.x <<";" << min_pt.y << ";" << min_pt.z <<std::endl;
std::cout << max_pt.x <<";" << min_pt.y << ";" << max_pt.z <<std::endl;
std::cout << max_pt.x <<";" << max_pt.y << ";" << min_pt.z <<std::endl;
std::cout << max_pt.x <<";" << max_pt.y << ";" << max_pt.z <<std::endl;
// Erzeugen einer Punktwolke mit Punkten, die zwischen den Extrema liegen (um anschließend eine dichte Oberfläche zu erzeugen)
pcl::PointCloud<pcl::PointXYZ>::Ptr Fillcloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointXYZ P1,P2,P3,P4;
P1.x=max_pt.x;P2.x=max_pt.x; P3.x=max_pt.x; P4.x=max_pt.x;
P1.y=min_pt.y;P2.y=min_pt.y; P3.y=max_pt.y; P4.y=max_pt.y;
P1.z=min_pt.z;P2.z=max_pt.z; P3.z=min_pt.z; P4.z=max_pt.z;
//P4.x=min_pt.x;P3.x=min_pt.x; P2.x=min_pt.x; P1.x=min_pt.x;
//P4.y=min_pt.y;P3.y=min_pt.y; P2.y=max_pt.y; P1.y=max_pt.y;
//P4.z=min_pt.z;P3.z=max_pt.z; P2.z=min_pt.z; P1.z=max_pt.z;
Fillcloud->push_back(P1);
Fillcloud->push_back(P2);
Fillcloud->push_back(P3);
Fillcloud->push_back(P4);
// Fillcloud->push_back (pcl::PointXYZ (min_pt.x, min_pt.y, min_pt.z)); // P1
// Fillcloud->push_back (pcl::PointXYZ (min_pt.x, min_pt.y, max_pt.z)); // P2
// Fillcloud->push_back (pcl::PointXYZ (min_pt.x, max_pt.y, min_pt.z)); // P3
// Fillcloud->push_back (pcl::PointXYZ (min_pt.x, max_pt.y, max_pt.z)); // P4 // Enweder alle x_max oder alle x_min nehmen WARUM?????
// Fillcloud->push_back (pcl::PointXYZ (max_pt.x, min_pt.y, min_pt.z));
// Fillcloud->push_back (pcl::PointXYZ (max_pt.x, min_pt.y, max_pt.z));
// Fillcloud->push_back (pcl::PointXYZ (max_pt.x, max_pt.y, min_pt.z));
// Fillcloud->push_back (pcl::PointXYZ (max_pt.x, max_pt.y, max_pt.z));
// Schleife, um BoundingBox-"Fläche" mit Punkten zu füllen (um ein Mesh erzeugen zu können
int AnzahlPunktehoch = 80;
int AnzahlPunktebreit = 400;
for( int ii = 0; ii < AnzahlPunktebreit+1; ii++ ){
Fillcloud->push_back( pcl::PointXYZ (P1.x+((P2.x-P1.x)/AnzahlPunktebreit)*ii , P1.y+((P2.y-P1.y)/AnzahlPunktebreit)*ii , P1.z+((P2.z-P1.z)/AnzahlPunktebreit)*ii) );
for( int jj = 0; jj < AnzahlPunktehoch+1; jj++ ){
Fillcloud->push_back( pcl::PointXYZ (P1.x+((P2.x-P1.x)/AnzahlPunktebreit)*ii + (P3.x-P1.x)/AnzahlPunktehoch*jj , P1.y+((P2.y-P1.y)/AnzahlPunktebreit)*ii + (P3.y-P1.y)/AnzahlPunktehoch*jj , P1.z+((P2.z-P1.z)/AnzahlPunktebreit)*ii + (P3.z-P1.z)/AnzahlPunktehoch*jj));
}
}
// Erzeugte Punktwolke, die die Extrema "verbindet" rücktransformieren
pcl::PointCloud<pcl::PointXYZ>::Ptr Fillcloud_transformiert (new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*Fillcloud,*Fillcloud_transformiert,tfinal,qfinal);
// Fillcloud wird zum Viewer hinzugefügt; "spin" auskommentieren wenn nicht jedes Fenster einzeln weggeklickt werden soll
//viewer->addPointCloud(Fillcloud_transformiert);
//viewer->spin();
//Alle Clusterebenen, die vertikal sind werden in planes_cloud gespeichert
*planes_cloud+=*Fillcloud_transformiert;
// Aus aktuellen Punkten vtkPlaneSource erzeugen
vtkPlaneSource *plane= vtkPlaneSource::New();
vtkPlaneSource *plane2= vtkPlaneSource::New();
plane->SetOrigin(min_pt.x,min_pt.y,min_pt.z);
plane->SetPoint1(min_pt.x,min_pt.y,max_pt.z);
plane->SetPoint2(min_pt.x,max_pt.y,min_pt.z);
plane->SetXResolution(10);
//plane->SetResolution(10,200); // Set the number of x-y subdivisions in the plane
plane->Update();
plane2->SetOrigin(min_pt.x,min_pt.y,max_pt.z);
plane2->SetPoint1(min_pt.x,max_pt.y,max_pt.z);
plane2->SetPoint2(min_pt.x,max_pt.y,min_pt.z);
//plane->SetResolution(1,1);
plane2->Update();
// appender->AddInputConnection(plane->GetOutputPort());
// appender->Update();
// actor->SetMapper(mapper);
// // Aktuelle Plane zu PlaneCollection hinzufügen
// PlaneCollection->AddItem(plane->GetOutputPort());
// PlaneCollection->AddItem(plane);
// mapper->SetInputConnection(plane->GetOutputPort());
// plane->Delete();
// mapper->SetInputConnection(plane->GetOutputPort());
// actor->SetMapper(mapper);
// renderer->AddActor(plane);
// vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
// polydata->SetVerts();
// polyplane->ShallowCopy(plane->GetOutput());
// appendFilter->AddInputConnection(polyplane->GetProducerPort());
//appendFilter->AddInput(plane->GetOutput());
//appendFilter->Update();
//appendFilter->AddInput(plane2->GetOutput());
//appendFilter->Update();
b=b+1;
}
}
}
// PlanesCollection als STL abspeichern
//vtkSmartPointer<vtkSTLWriter> schreiber = vtkSmartPointer<vtkSTLWriter>::New();
//schreiber->SetInput(appendFilter->GetOutput());
//schreiber->SetFileName("funktioniert nicht!");
//schreiber->Update();
//schreiber->Write();
// schreiber->SetInputConnection(plane->GetOutputPort());
// schreiber->SetFileName("");
// schreiber->SetInput(appender->GetOutput());
// //schreiber->SetFileTypeToASCII();
// schreiber->Write();
// Ausgabe
std::cout<<"Es wurden "<<b<<" Flächen in Planes_cloud.pcd geschrieben"<<endl;
writer.write("Planes_cloud.pcd",*planes_cloud,false);
std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;
std::cout << "First cluster has " << clusters[0].indices.size () << " points." << endl;
std::cout << "These are the indices of the points of the initial" <<
std::endl << "cloud that belong to the first cluster:" << std::endl;
int counter = 0;
while (counter < 5 || counter > clusters[0].indices.size ())
{
std::cout << clusters[0].indices[counter] << std::endl;
counter++;
}
// planes_cloud zu einer Oberfläche konvertieren u. als stl oder vtk speichern!
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals_triangles (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_triangles (new pcl::search::KdTree<pcl::PointXYZ>);
tree_triangles->setInputCloud(planes_cloud);
n.setInputCloud(planes_cloud);
n.setSearchMethod(tree_triangles);
n.setKSearch(20);
n.compute(*normals_triangles);
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields (*planes_cloud,*normals_triangles,*cloud_with_normals);
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud(cloud_with_normals);
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
pcl::PolygonMesh triangles;
gp3.setSearchRadius(0.4);// Ursprünglich 0.025
gp3.setMu(5.0); // Ursprünglich 2.5
gp3.setMaximumNearestNeighbors(20); // davor 100
gp3.setMaximumSurfaceAngle(M_PI/4); // ursprünglich: M_PI/4
gp3.setMinimumAngle(M_PI/18);
gp3.setMaximumAngle(2*M_PI/1); // ursprunglich: 2*M_PI/3
gp3.setNormalConsistency(false);
gp3.setInputCloud(cloud_with_normals);
gp3.setSearchMethod(tree2);
gp3.reconstruct(triangles);
std::vector<int> parts = gp3.getPartIDs();
std::vector<int> states = gp3.getPointStates();
// Oberfläche als STL-Modell abspeichern
pcl::io::savePolygonFileSTL("Arena_24_7_2014.stl", triangles);
//pcl::io::saveVTKFile("mesh.vtk", triangles);
// Farbliche Visualisierung der segmentierten Punktwolke
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
pcl::visualization::CloudViewer viewer ("Cluster viewer");
viewer.showCloud(colored_cloud);
viewer.runOnVisualizationThreadOnce(background_color);
while (!viewer.wasStopped ())
{
}
/*
pcl::PointCloud <pcl::PointXYZ>::Ptr cluster_cloud=clusters[0].indices;
pcl::visualization::CloudViewer viewer ("Cluster viewer");
viewer.showCloud(cluster_cloud);
while (!viewer.wasStopped())
{
}
*/
return (0);
}
void PwnTracker::processFrame(const DepthImage& depthImage_,
const Eigen::Isometry3f& sensorOffset_,
const Eigen::Matrix3f& cameraMatrix_,
const Eigen::Isometry3f& initialGuess){
int r,c;
Eigen::Matrix3f scaledCameraMatrix = cameraMatrix_;
_currentCloudOffset = sensorOffset_;
_currentCameraMatrix = cameraMatrix_;
_currentDepthImage = depthImage_;
_currentCloud = makeCloud(r,c,scaledCameraMatrix,_currentCloudOffset,_currentDepthImage);
bool newFrame = false;
bool newRelation = false;
PwnTrackerFrame* currentTrackerFrame=0;
Eigen::Isometry3d relationMean;
if (_previousCloud){
_aligner->setCurrentSensorOffset(_currentCloudOffset);
_aligner->setCurrentFrame(_currentCloud);
_aligner->setReferenceSensorOffset(_previousCloudOffset);
_aligner->setReferenceFrame(_previousCloud);
_aligner->correspondenceFinder()->setImageSize(r,c);
PinholePointProjector* alprojector=dynamic_cast<PinholePointProjector*>(_aligner->projector());
alprojector->setCameraMatrix(scaledCameraMatrix);
alprojector->setImageSize(r,c);
Eigen::Isometry3f guess=_previousCloudTransform.inverse()*_globalT*initialGuess;
_aligner->setInitialGuess(guess);
double t0, t1;
t0 = g2o::get_time();
_aligner->align();
t1 = g2o::get_time();
std::cout << "Time: " << t1 - t0 << " seconds " << std::endl;
cerr << "inliers: " << _aligner->inliers() << endl;
// cerr << "chi2: " << _aligner->error() << endl;
// cerr << "chi2/inliers: " << _aligner->error()/_aligner->inliers() << endl;
// cerr << "initialGuess: " << t2v(guess).transpose() << endl;
// cerr << "transform : " << t2v(_aligner->T()).transpose() << endl;
if (_aligner->inliers()>0){
_globalT = _previousCloudTransform*_aligner->T();
//cerr << "TRANSFORM FOUND" << endl;
} else {
//cerr << "FAILURE" << endl;
_globalT = _globalT*guess;
}
convertScalar(relationMean, _aligner->T());
if (! (_counter%50) ) {
Eigen::Matrix3f R = _globalT.linear();
Eigen::Matrix3f E = R.transpose() * R;
E.diagonal().array() -= 1;
_globalT.linear() -= 0.5 * R * E;
}
_globalT.matrix().row(3) << 0,0,0,1;
newAlignmentCallbacks(_globalT, _aligner->T(), _aligner->inliers(), _aligner->error());
int maxInliers = r*c;
float inliersFraction = (float) _aligner->inliers()/(float) maxInliers;
cerr << "inliers/maxinliers/fraction: " << _aligner->inliers() << "/" << maxInliers << "/" << inliersFraction << endl;
if (inliersFraction<_newFrameInliersFraction){
newFrame = true;
newRelation = true;
// char filename[1024];
// sprintf (filename, "frame-%05d.pwn", _numKeyframes);
// frame->save(filename,1,true,_globalT);
_numKeyframes ++;
if (!_cache)
delete _previousCloud;
else{
_previousCloudHandle.release();
}
//_cache->unlock(_previousTrackerFrame);
// _aligner->setReferenceSensorOffset(_currentCloudOffset);
// _aligner->setReferenceFrame(_currentCloud);
_previousCloud = _currentCloud;
_previousCloudTransform = _globalT;
// cerr << "new frame added (" << _numKeyframes << ")" << endl;
// cerr << "inliers: " << _aligner->inliers() << endl;
// cerr << "maxInliers: " << maxInliers << endl;
// cerr << "chi2: " << _aligner->error() << endl;
// cerr << "chi2/inliers: " << _aligner->error()/_aligner->inliers() << endl;
// cerr << "initialGuess: " << t2v(guess).transpose() << endl;
// cerr << "transform : " << t2v(_aligner->T()).transpose() << endl;
// cerr << "globalTransform : " << t2v(_globalT).transpose() << endl;
} else { // previous frame but offset is small
delete _currentCloud;
_currentCloud = 0;
}
} else { // first frame
//ser.writeObject(*manager);
newFrame = true;
// _aligner->setReferenceSensorOffset(_currentCloudOffset);
// _aligner->setReferenceFrame(_currentCloud);
_previousCloud = _currentCloud;
_previousCloudTransform = _globalT;
_previousCloudOffset = _currentCloudOffset;
_numKeyframes ++;
/*Eigen::Isometry3f t = _globalT;
geometry_msgs::Point p;
p.x = t.translation().x();
p.y = t.translation().y();
p.z = t.translation().z();
m_odometry.points.push_back(p);
*/
}
_counter++;
if (newFrame) {
//cerr << "maing new frame, previous: " << _previousTrackerFrame << endl;
currentTrackerFrame = new PwnTrackerFrame(_manager);
//currentTrackerFrame->cloud.set(cloud);
currentTrackerFrame->cloud=_currentCloud;
currentTrackerFrame->sensorOffset = _currentCloudOffset;
boss_map::ImageBLOB* depthBLOB=new boss_map::ImageBLOB();
DepthImage_convert_32FC1_to_16UC1(depthBLOB->cvImage(),_currentDepthImage);
//depthImage.toCvMat(depthBLOB->cvImage());
depthBLOB->adjustFormat();
currentTrackerFrame->depthImage.set(depthBLOB);
boss_map::ImageBLOB* normalThumbnailBLOB=new boss_map::ImageBLOB();
boss_map::ImageBLOB* depthThumbnailBLOB=new boss_map::ImageBLOB();
makeThumbnails(depthThumbnailBLOB->cvImage(), normalThumbnailBLOB->cvImage(), _currentCloud,
_currentDepthImage.rows,
_currentDepthImage.cols,
_currentCloudOffset,
_currentCameraMatrix,
0.1);
normalThumbnailBLOB->adjustFormat();
depthThumbnailBLOB->adjustFormat();
currentTrackerFrame->depthThumbnail.set(depthThumbnailBLOB);
currentTrackerFrame->normalThumbnail.set(normalThumbnailBLOB);
currentTrackerFrame->cameraMatrix = _currentCameraMatrix;
currentTrackerFrame->imageRows = _currentDepthImage.rows;
currentTrackerFrame->imageCols = _currentDepthImage.cols;
Eigen::Isometry3d _iso;
convertScalar(_iso, _globalT);
currentTrackerFrame->setTransform(_iso);
convertScalar(currentTrackerFrame->sensorOffset, _currentCloudOffset);
currentTrackerFrame->setSeq(_seq++);
_manager->addNode(currentTrackerFrame);
//cerr << "AAAA" << endl;
if (_cache)
_currentCloudHandle=_cache->get(currentTrackerFrame);
//cerr << "BBBB" << endl;
//_cache->lock(currentTrackerFrame);
newFrameCallbacks(currentTrackerFrame);
currentTrackerFrame->depthThumbnail.set(0);
currentTrackerFrame->normalThumbnail.set(0);
currentTrackerFrame->depthImage.set(0);
}
if (newRelation) {
PwnTrackerRelation* rel = new PwnTrackerRelation(_manager);
rel->setTransform(relationMean);
Matrix6d omega;
convertScalar(omega, _aligner->omega());
omega.setIdentity();
omega *= 100;
rel->setInformationMatrix(omega);
rel->setTo(currentTrackerFrame);
rel->setFrom(_previousTrackerFrame);
//cerr << "saved relation" << _previousTrackerFrame << " " << currentTrackerFrame << endl;
_manager->addRelation(rel);
newRelationCallbacks(rel);
//ser.writeObject(*rel);
}
if (currentTrackerFrame) {
_previousTrackerFrame = currentTrackerFrame;
}
}
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;
}