bool GridMap::getPosition3d(const std::string& type, const Eigen::Array2i& index, Eigen::Vector3d& position) const
{
if (!isValid(index)) return false;
Vector2d position2d;
getPosition(index, position2d);
position.head(2) = position2d;
position.z() = at(type, index);
return true;
}
void LinearSystem::_BuildSystem(
LinearSystem* pLS,
const unsigned int& StartU,
const unsigned int& EndU,
const unsigned int& StartV,
const unsigned int& EndV
)
{
// Jacobian
Eigen::Matrix<double, 1, 6> BigJ;
Eigen::Matrix<double, 1, 6> J;
BigJ.setZero();
J.setZero();
// Errors
double e;
double SSD = 0;
double Error = 0;
unsigned int ErrorPts = 0;
for( int ii = StartV; ii < EndV; ii++ ) {
for( int jj = StartU; jj < EndU; jj++ ) {
// variables
Eigen::Vector2d Ur; // pixel position
Eigen::Vector3d Pr, Pv; // 3d point
Eigen::Vector4d Ph; // homogenized point
// check if pixel is contained in our model (i.e. has depth)
if( pLS->m_vVirtDepth[ii * pLS->m_nImgWidth + jj] == 0 ) {
continue;
}
// --------------------- first term 1x2
// evaluate 'a' = L[ Trv * Linv( Uv ) ]
// back project to virtual camera's reference frame
// this already brings points to robotics reference frame
Pv = pLS->_BackProject( jj, ii, pLS->m_vVirtDepth[ii * pLS->m_nImgWidth + jj] );
// convert to homogeneous coordinate
Ph << Pv, 1;
// transform point to reference camera's frame
// Pr = Trv * Pv
Ph = pLS->m_dTrv * Ph;
Pr = Ph.head( 3 );
// project onto reference camera
Eigen::Vector3d Lr;
Lr = pLS->_Project( Pr );
Ur = Lr.head( 2 );
Ur = Ur / Lr( 2 );
// check if point falls in camera's field of view
if( (Ur( 0 ) <= 1) || (Ur( 0 ) >= pLS->m_nImgWidth - 2) || (Ur( 1 ) <= 1)
|| (Ur( 1 ) >= pLS->m_nImgHeight - 2) ) {
continue;
}
// finite differences
float TopPix = pLS->_Interpolate( Ur( 0 ), Ur( 1 ) - 1, pLS->m_vRefImg );
float BotPix = pLS->_Interpolate( Ur( 0 ), Ur( 1 ) + 1, pLS->m_vRefImg );
float LeftPix = pLS->_Interpolate( Ur( 0 ) - 1, Ur( 1 ), pLS->m_vRefImg );
float RightPix = pLS->_Interpolate( Ur( 0 ) + 1, Ur( 1 ), pLS->m_vRefImg );
Eigen::Matrix<double, 1, 2> Term1;
Term1( 0 ) = (RightPix - LeftPix) / 2.0;
Term1( 1 ) = (BotPix - TopPix) / 2.0;
// --------------------- second term 2x3
// evaluate 'b' = Trv * Linv( Uv )
// this was already calculated for Term1
// fill matrix
// 1/c 0 -a/c^2
// 0 1/c -b/c^2
Eigen::Matrix<double, 2, 3> Term2;
double PowC = Lr( 2 ) * Lr( 2 );
Term2( 0, 0 ) = 1.0 / Lr( 2 );
Term2( 0, 1 ) = 0;
Term2( 0, 2 ) = -(Lr( 0 )) / PowC;
Term2( 1, 0 ) = 0;
Term2( 1, 1 ) = 1.0 / Lr( 2 );
Term2( 1, 2 ) = -(Lr( 1 )) / PowC;
Term2 = Term2 * pLS->m_Kr;
// --------------------- third term 3x1
// we need Pv in homogenous coordinates
Ph << Pv, 1;
Eigen::Vector4d Term3i;
// last row of Term3 is truncated since it is always 0
Eigen::Vector3d Term3;
// fill Jacobian with T generators
Term3i = pLS->m_dTrv * pLS->m_Gen[0] * Ph;
Term3 = Term3i.head( 3 );
J( 0, 0 ) = Term1 * Term2 * Term3;
pLS->m_vJImgX[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 0);
Term3i = pLS->m_dTrv * pLS->m_Gen[1] * Ph;
Term3 = Term3i.head( 3 );
J( 0, 1 ) = Term1 * Term2 * Term3;
pLS->m_vJImgY[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 1);
Term3i = pLS->m_dTrv * pLS->m_Gen[2] * Ph;
Term3 = Term3i.head( 3 );
J( 0, 2 ) = Term1 * Term2 * Term3;
pLS->m_vJImgZ[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 2);
Term3i = pLS->m_dTrv * pLS->m_Gen[3] * Ph;
Term3 = Term3i.head( 3 );
J( 0, 3 ) = Term1 * Term2 * Term3;
pLS->m_vJImgP[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 3);
Term3i = pLS->m_dTrv * pLS->m_Gen[4] * Ph;
Term3 = Term3i.head( 3 );
J( 0, 4 ) = Term1 * Term2 * Term3;
pLS->m_vJImgQ[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 4);
Term3i = pLS->m_dTrv * pLS->m_Gen[5] * Ph;
Term3 = Term3i.head( 3 );
J( 0, 5 ) = Term1 * Term2 * Term3;
pLS->m_vJImgR[(StartV + ii) * pLS->m_nImgWidth + (StartU) + jj] = J(0, 5);
// accumulate Jacobian
BigJ += J;
// accumulate SSD error
e = pLS->_Interpolate( Ur( 0 ), Ur( 1 ), pLS->m_vRefImg )
- pLS->m_vVirtImg[ii * pLS->m_nImgWidth + jj];
SSD += e * e;
// calculate normalized error
Error += fabs( pLS->_Interpolate( Ur( 0 ), Ur( 1 ), pLS->m_vRefImg )
- pLS->m_vVirtImg[ii * pLS->m_nImgWidth + jj] );
ErrorPts++;
}
}
// update global LHS and RHS
// ---------- start contention zone
pLS->m_Mutex.lock();
pLS->m_J += BigJ;
pLS->m_dSSD += SSD;
pLS->m_dError += Error;
pLS->m_nErrorPts += ErrorPts;
pLS->m_Mutex.unlock();
// ---------- end contention zone
}
void NuTo::StructureBase::ConstraintLinearEquationNodeToElementCreate(int rNode, int rElementGroup, NuTo::Node::eDof,
const double rTolerance,
Eigen::Vector3d rNodeCoordOffset)
{
const int dim = GetDimension();
Eigen::VectorXd queryNodeCoords = NodeGetNodePtr(rNode)->Get(Node::eDof::COORDINATES);
queryNodeCoords = queryNodeCoords + rNodeCoordOffset.head(dim);
std::vector<int> elementGroupIds = GroupGetMemberIds(rElementGroup);
ElementBase* elementPtr = nullptr;
Eigen::VectorXd elementNaturalNodeCoords;
bool nodeInElement = false;
for (auto const& eleId : elementGroupIds)
{
elementPtr = ElementGetElementPtr(eleId);
// Coordinate interpolation must be linear so the shape function derivatives are constant!
assert(elementPtr->GetInterpolationType().Get(Node::eDof::COORDINATES).GetTypeOrder() ==
Interpolation::eTypeOrder::EQUIDISTANT1);
const Eigen::MatrixXd& derivativeShapeFunctionsGeometryNatural =
elementPtr->GetInterpolationType()
.Get(Node::eDof::COORDINATES)
.DerivativeShapeFunctionsNatural(
Eigen::VectorXd::Zero(dim)); // just as _some_ point, as said, constant
// real coordinates of every node in rElement
Eigen::VectorXd elementNodeCoords = elementPtr->ExtractNodeValues(NuTo::Node::eDof::COORDINATES);
switch (mDimension)
{
case 2:
{
Eigen::Matrix2d invJacobian =
dynamic_cast<ContinuumElement<2>*>(elementPtr)
->CalculateJacobian(derivativeShapeFunctionsGeometryNatural, elementNodeCoords)
.inverse();
elementNaturalNodeCoords = invJacobian * (queryNodeCoords - elementNodeCoords.head(2));
}
break;
case 3:
{
Eigen::Matrix3d invJacobian =
dynamic_cast<ContinuumElement<3>*>(elementPtr)
->CalculateJacobian(derivativeShapeFunctionsGeometryNatural, elementNodeCoords)
.inverse();
elementNaturalNodeCoords = invJacobian * (queryNodeCoords - elementNodeCoords.head(3));
}
break;
default:
throw NuTo::Exception(std::string(__PRETTY_FUNCTION__) + ": \t Only implemented for 2D and 3D");
}
if ((elementNaturalNodeCoords.array() > -rTolerance).all() and
elementNaturalNodeCoords.sum() <= 1. + rTolerance)
{
nodeInElement = true;
break;
}
}
if (not nodeInElement)
{
GetLogger() << "Natural node coordinates: \n" << elementNaturalNodeCoords << "\n";
throw Exception(__PRETTY_FUNCTION__, "Node is not inside any element.");
}
auto shapeFunctions =
elementPtr->GetInterpolationType().Get(Node::eDof::DISPLACEMENTS).ShapeFunctions(elementNaturalNodeCoords);
std::vector<Constraint::Equation> equations(dim); // default construction of Equation with rhs = Constant = 0
for (int iDim = 0; iDim < dim; ++iDim)
{
equations[iDim].AddTerm(Constraint::Term(*NodeGetNodePtr(rNode), iDim, 1.));
}
for (int iNode = 0; iNode < shapeFunctions.rows(); ++iNode)
{
int localNodeId = elementPtr->GetInterpolationType().Get(Node::eDof::DISPLACEMENTS).GetNodeIndex(iNode);
auto globalNode = elementPtr->GetNode(localNodeId, Node::eDof::DISPLACEMENTS);
// std::cout << "globalNodeId \t" << globalNodeId << std::endl;
double coefficient = -shapeFunctions(iNode, 0);
for (int iDim = 0; iDim < dim; ++iDim)
equations[iDim].AddTerm(Constraint::Term(*globalNode, iDim, coefficient));
}
Constraints().Add(Node::eDof::DISPLACEMENTS, equations);
}