/**
* @function heuristicCost
* @brief
*/
double M_RRT::heuristicCost( Eigen::VectorXd node )
{
Eigen::Transform<double, 3, Eigen::Affine> T;
// Calculate the EE position
robinaLeftArm_fk( node, TWBase, Tee, T );
Eigen::VectorXd trans_ee = T.translation();
Eigen::VectorXd x_ee = T.rotation().col(0);
Eigen::VectorXd y_ee = T.rotation().col(1);
Eigen::VectorXd z_ee = T.rotation().col(2);
Eigen::VectorXd GH = ( goalPosition - trans_ee );
double fx1 = GH.norm() ;
GH = GH/GH.norm();
double fx2 = abs( GH.dot( x_ee ) - 1 );
double fx3 = abs( GH.dot( z_ee ) );
double heuristic = w1*fx1 + w2*fx2 + w3*fx3;
return heuristic;
}
void TranslationRotation3D::setF(const std::vector<double> &F_in) {
if (F_in.size() != 16)
throw std::runtime_error(
"TranslationRotation3D::setF: F_in requires 16 elements");
if ((F_in.at(12) != 0.0) || (F_in.at(13) != 0.0) || (F_in.at(14) != 0.0) ||
(F_in.at(15) != 1.0))
throw std::runtime_error(
"TranslationRotation3D::setF: bottom row of F_in should be [0 0 0 1]");
Eigen::Map<const Eigen::Matrix<double, 4, 4, Eigen::RowMajor> > F_in_eig(
F_in.data());
Eigen::Transform<double, 3, Eigen::Affine> F;
F = F_in_eig;
double tmpT[3];
Eigen::Map<Eigen::Vector3d> tra_eig(tmpT);
tra_eig = F.translation();
double tmpR_mat[9];
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_eig(tmpR_mat);
rot_eig = F.rotation();
setT(tmpT);
setR_mat(tmpR_mat);
updateR_mat(); // for stability
}
template <typename PointT, typename Scalar> inline PointT
pcl::transformPointWithNormal (const PointT &point,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
PointT ret = point;
ret.getVector3fMap () = transform * point.getVector3fMap ();
ret.getNormalVector3fMap () = transform.rotation () * point.getNormalVector3fMap ();
return (ret);
}
void toGeometryMsg(geometry_msgs::Transform& out, const Eigen::Transform<double, 3, TransformType>& pose) {
// convert accumulated_pose_ to transformStamped
Eigen::Quaterniond rot_quat(pose.rotation());
out.translation.x = pose.translation().x();
out.translation.y = pose.translation().y();
out.translation.z = pose.translation().z();
out.rotation.x = rot_quat.x();
out.rotation.y = rot_quat.y();
out.rotation.z = rot_quat.z();
out.rotation.w = rot_quat.w();
}
//DEPRECATED???
double
NDTMatcherFeatureD2D::scoreNDT(std::vector<NDTCell*> &sourceNDT, NDTMap &targetNDT,
Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T)
{
NUMBER_OF_ACTIVE_CELLS = 0;
double score_here = 0;
double det = 0;
bool exists = false;
NDTCell *cell;
Eigen::Matrix3d covCombined, icov;
Eigen::Vector3d meanFixed;
Eigen::Vector3d meanMoving;
Eigen::Matrix3d R = T.rotation();
std::vector<std::pair<unsigned int, double> > scores;
for(unsigned int j=0; j<_corr.size(); j++)
{
unsigned int i = _corr[j].second;
if (_corr[j].second >= (int)sourceNDT.size())
{
std::cout << "second correspondance : " << _corr[j].second << ", " << sourceNDT.size() << std::endl;
}
if (sourceNDT[i] == NULL)
{
std::cout << "sourceNDT[i] == NULL!" << std::endl;
}
meanMoving = T*sourceNDT[i]->getMean();
cell = targetNDT.getCellIdx(_corr[j].first);
{
if(cell == NULL)
{
std::cout << "cell== NULL!!!" << std::endl;
}
else
{
if(cell->hasGaussian_)
{
meanFixed = cell->getMean();
covCombined = cell->getCov() + R.transpose()*sourceNDT[i]->getCov()*R;
covCombined.computeInverseAndDetWithCheck(icov,det,exists);
if(!exists) continue;
double l = (meanMoving-meanFixed).dot(icov*(meanMoving-meanFixed));
if(l*0 != 0) continue;
if(l > 120) continue;
double sh = -lfd1*(exp(-lfd2*l/2));
if(fabsf(sh) > 1e-10)
{
NUMBER_OF_ACTIVE_CELLS++;
}
scores.push_back(std::pair<unsigned int, double>(j, sh));
score_here += sh;
//score_here += l;
}
}
}
}
if (_trimFactor == 1.)
{
return score_here;
}
else
{
// Determine the score value
if (scores.empty()) // TODO, this happens(!), why??!??
return score_here;
score_here = 0.;
unsigned int index = static_cast<unsigned int>(_trimFactor * (scores.size() - 1));
// std::nth_element (scores.begin(), scores.begin()+index, scores.end(), sort_scores()); //boost::bind(&std::pair<unsigned int, double>::second, _1) < boost::bind(&std::pair<unsigned int, double>::second, _2));
std::nth_element (scores.begin(), scores.begin()+index, scores.end(), boost::bind(&std::pair<unsigned int, double>::second, _1) < boost::bind(&std::pair<unsigned int, double>::second, _2));
std::fill(_goodCorr.begin(), _goodCorr.end(), false);
// std::cout << "_goodCorr.size() : " << _goodCorr.size() << " scores.size() : " << scores.size() << " index : " << index << std::endl;
for (unsigned int i = 0; i < _goodCorr.size(); i++)
{
if (i <= index)
{
score_here += scores[i].second;
_goodCorr[scores[i].first] = true;
}
}
return score_here;
}
}