// apply the distmesh algorithm
std::tuple<Eigen::ArrayXXd, Eigen::ArrayXXi> distmesh::distmesh(
Functional const& distanceFunction, double const initialPointDistance,
Functional const& elementSizeFunction, Eigen::Ref<Eigen::ArrayXXd const> const boundingBox,
Eigen::Ref<Eigen::ArrayXXd const> const fixedPoints) {
// determine dimension of mesh
unsigned const dimension = boundingBox.cols();
// create initial distribution in bounding box
Eigen::ArrayXXd points = utils::createInitialPoints(distanceFunction,
initialPointDistance, elementSizeFunction, boundingBox, fixedPoints);
// create initial triangulation
Eigen::ArrayXXi triangulation = triangulation::delaunay(points);
// create buffer to store old point locations to calculate
// retriangulation and stop criterion
Eigen::ArrayXXd retriangulationCriterionBuffer = Eigen::ArrayXXd::Constant(
points.rows(), points.cols(), INFINITY);
Eigen::ArrayXXd stopCriterionBuffer = Eigen::ArrayXXd::Zero(
points.rows(), points.cols());
// main distmesh loop
Eigen::ArrayXXi edgeIndices;
for (unsigned step = 0; step < constants::maxSteps; ++step) {
// retriangulate if point movement is above threshold
if ((points - retriangulationCriterionBuffer).square().rowwise().sum().sqrt().maxCoeff() >
constants::retriangulationThreshold * initialPointDistance) {
// update triangulation
triangulation = triangulation::delaunay(points);
// reject triangles with circumcenter outside of the region
Eigen::ArrayXXd circumcenter = Eigen::ArrayXXd::Zero(triangulation.rows(), dimension);
for (int point = 0; point < triangulation.cols(); ++point) {
circumcenter += utils::selectIndexedArrayElements<double>(
points, triangulation.col(point)) / triangulation.cols();
}
triangulation = utils::selectMaskedArrayElements<int>(triangulation,
distanceFunction(circumcenter) < -constants::geometryEvaluationThreshold * initialPointDistance);
// find unique edge indices
edgeIndices = utils::findUniqueEdges(triangulation);
// store current points positions
retriangulationCriterionBuffer = points;
}
// calculate edge vectors and their length
auto const edgeVector = (utils::selectIndexedArrayElements<double>(points, edgeIndices.col(0)) -
utils::selectIndexedArrayElements<double>(points, edgeIndices.col(1))).eval();
auto const edgeLength = edgeVector.square().rowwise().sum().sqrt().eval();
// evaluate elementSizeFunction at midpoints of edges
auto const desiredElementSize = elementSizeFunction(0.5 *
(utils::selectIndexedArrayElements<double>(points, edgeIndices.col(0)) +
utils::selectIndexedArrayElements<double>(points, edgeIndices.col(1)))).eval();
// calculate desired edge length
auto const desiredEdgeLength = (desiredElementSize * (1.0 + 0.4 / std::pow(2.0, dimension - 1)) *
std::pow((edgeLength.pow(dimension).sum() / desiredElementSize.pow(dimension).sum()),
1.0 / dimension)).eval();
// calculate force vector for each edge
auto const forceVector = (edgeVector.colwise() *
((desiredEdgeLength - edgeLength) / edgeLength).max(0.0)).eval();
// store current points positions
stopCriterionBuffer = points;
// move points
for (int edge = 0; edge < edgeIndices.rows(); ++edge) {
if (edgeIndices(edge, 0) >= fixedPoints.rows()) {
points.row(edgeIndices(edge, 0)) += constants::deltaT * forceVector.row(edge);
}
if (edgeIndices(edge, 1) >= fixedPoints.rows()) {
points.row(edgeIndices(edge, 1)) -= constants::deltaT * forceVector.row(edge);
}
}
// project points outside of domain to boundary
utils::projectPointsToBoundary(distanceFunction, initialPointDistance, points);
// stop, when maximum points movement is below threshold
if ((points - stopCriterionBuffer).square().rowwise().sum().sqrt().maxCoeff() <
constants::pointsMovementThreshold * initialPointDistance) {
break;
}
}
return std::make_tuple(points, triangulation);
}
bool mrpt::vision::pnp::CPnP::dls(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){
try{
#if MRPT_HAS_OPENCV==1
// Input 2d/3d correspondences and camera intrinsic matrix
Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
// Check for consistency of input matrix dimensions
if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
throw(2);
else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
throw(3);
if(obj_pts.rows() < obj_pts.cols())
{
cam_in_eig=cam_intrinsic.transpose();
img_pts_eig=img_pts.transpose().block(0,0,n,2);
obj_pts_eig=obj_pts.transpose();
}
else
{
cam_in_eig=cam_intrinsic;
img_pts_eig=img_pts.block(0,0,n,2);
obj_pts_eig=obj_pts;
}
// Output pose
Eigen::Matrix3d R_eig;
Eigen::MatrixXd t_eig;
// Compute pose
cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv(3,3,CV_32F), t_cv(3,1,CV_32F);
cv::eigen2cv(cam_in_eig, cam_in_cv);
cv::eigen2cv(img_pts_eig, img_pts_cv);
cv::eigen2cv(obj_pts_eig, obj_pts_cv);
mrpt::vision::pnp::dls d(obj_pts_cv, img_pts_cv);
bool ret = d.compute_pose(R_cv,t_cv);
cv::cv2eigen(R_cv, R_eig);
cv::cv2eigen(t_cv, t_eig);
Eigen::Quaterniond q(R_eig);
pose_mat << t_eig,q.vec();
return ret;
#else
throw(-1)
#endif
}
catch(int e)
{
switch(e)
{
case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
case 2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
}
return false;
}
}
