void testSolverSpeed(const std::string inputFilePath,const std::string outputFilePath,const int sizeThreshold,std::string solverType,user_IndexTree & usrTree){
Eigen::MatrixXd inputMatrix = readBinaryIntoMatrixXd(inputFilePath);
int matrixSize = inputMatrix.rows();
Eigen::VectorXd exactSoln = Eigen::VectorXd::LinSpaced(Eigen::Sequential,matrixSize,-2,2);
Eigen::VectorXd inputF = inputMatrix * exactSoln;
HODLR_Matrix test_HODLR(inputMatrix,sizeThreshold,usrTree);
std::ofstream outputFile_Error;
std::ofstream outputFile_Speed;
std::string errorFileName = outputFilePath + "errorVsTolerance";
std::string speedFileName = outputFilePath + "speedVsTolerance";
outputFile_Error.open(errorFileName.c_str());
outputFile_Speed.open(speedFileName.c_str());
double accuracyVal[] = {1e-5,1e-6,1e-7,1e-8};
std::vector<double> accuracy(accuracyVal, accuracyVal + sizeof(accuracyVal) / sizeof(double));
for (unsigned int i = 0; i < accuracy.size();i++){
test_HODLR.set_LRTolerance(accuracy[i]);
Eigen::VectorXd solverSoln;
if (solverType == "recLU"){
solverSoln = test_HODLR.recLU_Solve(inputF);
outputFile_Speed<<accuracy[i]<<" "<<test_HODLR.get_recLU_TotalTime()<<std::endl;
}else if (solverType == "extendedSp"){
solverSoln = test_HODLR.extendedSp_Solve(inputF);
outputFile_Speed<<accuracy[i]<<" "<<test_HODLR.get_extendedSp_TotalTime()<<std::endl;
}
Eigen::VectorXd difference = solverSoln - exactSoln;
double relError = difference.norm()/exactSoln.norm();
outputFile_Error<<accuracy[i]<<" "<<relError<<std::endl;
}
outputFile_Error.close();
outputFile_Speed.close();
}
bool UpperBodyPlanner::checkTrajectory(const moveit_msgs::RobotTrajectory& input_trajectory, moveit_msgs::RobotTrajectory& output_trajectory) {
moveit_msgs::RobotTrajectory check_trajectory = input_trajectory;
int input_traj_size = input_trajectory.joint_trajectory.points.size();
if (input_traj_size == 0) {
ROS_ERROR("No points in the trajectory.");
singularity_check = true;
return false;
}
else if (input_traj_size == 1) {
ROS_INFO("1 point in the trajectory.");
output_trajectory = check_trajectory;
singularity_check = true;
return true;
}
int i = 0;
singularity_check = true;
while (i < (check_trajectory.joint_trajectory.points.size() - 1)) {
//std::cout << "Checking point number is: " << i << std::endl;
Eigen::VectorXd front = stdVec2EigenXd(check_trajectory.joint_trajectory.points[i].positions);
Eigen::VectorXd back = stdVec2EigenXd(check_trajectory.joint_trajectory.points[i + 1].positions);
Eigen::VectorXd difference = front - back;
if (difference.norm() == 0) {
//std::cout << "The point " << i << " and " << i + 1 << " is too close." << std::endl;
//std::cout << "Erase point " << i + 1 << " points" << std::endl;
check_trajectory.joint_trajectory.points.erase(check_trajectory.joint_trajectory.points.begin() + (i + 1));
}
else {
if (difference.norm() >= 0.5) {
ROS_WARN("Hit singularity");
singularity_check = false;
}
i++;
}
}
//double fix_time = check_trajectory.joint_trajectory.points[0].time_from_start.toSec();
double fix_time = path_step / move_velocity;
Eigen::VectorXd velocity_cmd = Eigen::VectorXd::Zero(7);
double start_t = 0;
for (int j = 0; j < check_trajectory.joint_trajectory.points.size(); j++) {
if (j == 0) {
start_t = start_t + 2 * fix_time;
velocity_cmd = Eigen::VectorXd::Zero(7);
}
else if (j == (check_trajectory.joint_trajectory.points.size() - 1)) {
start_t = start_t + 2 * fix_time;
velocity_cmd = Eigen::VectorXd::Zero(7);
}
else {
Eigen::VectorXd current_joint_angle = stdVec2EigenXd(check_trajectory.joint_trajectory.points[j + 1].positions);
Eigen::VectorXd last_joint_angle = stdVec2EigenXd(check_trajectory.joint_trajectory.points[j].positions);
velocity_cmd = (current_joint_angle - last_joint_angle) / fix_time;
}
start_t = start_t + fix_time;
ros::Duration start_time((start_t));
check_trajectory.joint_trajectory.points[j].time_from_start = start_time;
check_trajectory.joint_trajectory.points[j].velocities = EigenXd2stdVec(velocity_cmd);
}
output_trajectory = check_trajectory;
return true;
}
/**
* @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;
}
/**
* @function GoToXYZ
*/
bool JTFollower::GoToXYZ( Eigen::VectorXd &_q,
Eigen::VectorXd _targetXYZ,
std::vector<Eigen::VectorXd> &_workspacePath ) {
Eigen::VectorXd dXYZ;
Eigen::VectorXd dConfig;
int iter;
mWorld->getRobot(mRobotId)->update();
//-- Initialize
dXYZ = ( _targetXYZ - GetXYZ(_q) ); // GetXYZ also updates the config to _q, so Jaclin use an updated value
iter = 0;
//printf("New call to GoToXYZ: dXYZ: %f \n", dXYZ.norm() );
while( dXYZ.norm() > mWorkspaceThresh && iter < mMaxIter ) {
//printf("XYZ Error: %f \n", dXYZ.norm() );
Eigen::MatrixXd Jt = GetPseudoInvJac(_q);
dConfig = Jt*dXYZ;
//printf("dConfig : %.3f \n", dConfig.norm() );
if( dConfig.norm() > mConfigStep ) {
double n = dConfig.norm();
dConfig = dConfig *(mConfigStep/n);
//printf("NEW dConfig : %.3f \n", dConfig.norm() );
}
_q = _q + dConfig;
_workspacePath.push_back( _q );
dXYZ = (_targetXYZ - GetXYZ(_q) );
iter++;
}
if( iter >= mMaxIter ) { return false; }
else { return true; }
}
/* Function: testACASolverConv1DUniformPts
* ---------------------------------------
* This function creates a convergence plot of solver relative error vs ACA tolerance for a dense self interaction matrix.
* The test dense matrix, is an interaction matrix arising from the self interaction of uniform points on a 1D interval.
* intervalMin : Lower bound of the 1D interval.
* intervalMax : Upper bound of the 1D interval.
* numPts : Number of interacting points (matrix size).
* diagValue : The diagonal entry value of the dense matrix.
* exactSoln : The test right hand side of the linear system.
* outputFileName : Path of the output file.
* kernel : Pointer to the kernel function.
* solverType : Type of HODLR solver. Default is recLU.
*/
void testACASolverConv1DUnifromPts(const double intervalMin,const double intervalMax, const int numPts, const double diagValue, Eigen::VectorXd exactSoln, std::string outputFileName, double (*kernel)(const double r),std::string solverType){
assert(intervalMax > intervalMin);
assert(numPts > 0);
assert(exactSoln.rows() == numPts);
int minTol = -5;
int maxTol = -10;
int sizeThreshold = 30;
Eigen::MatrixXd denseMatrix = makeMatrix1DUniformPts (intervalMin, intervalMax, intervalMin, intervalMax, numPts, numPts, diagValue, kernel);
Eigen::VectorXd RHS = denseMatrix * exactSoln;
HODLR_Matrix denseHODLR(denseMatrix, sizeThreshold);
std::ofstream outputFile;
outputFile.open(outputFileName.c_str());
for (int i = minTol; i >= maxTol; i--){
double tol = pow(10,i);
denseHODLR.set_LRTolerance(tol);
Eigen::VectorXd solverSoln;
if (solverType == "recLU")
solverSoln = denseHODLR.recLU_Solve(RHS);
if (solverType == "extendedSp")
solverSoln = denseHODLR.extendedSp_Solve(RHS);
Eigen::VectorXd residual = solverSoln-exactSoln;
double relError = residual.norm()/exactSoln.norm();
outputFile<<tol<<" "<<relError<<std::endl;
}
outputFile.close();
}
Residuals(const Problem& problem, const Omega& omega, const Psi& psi)
: normOfC{problem.c.norm()},
normOfB{problem.b.norm()},
AXPlusS{problem.A * omega.x + psi.s},
ATransposeY{problem.A.transpose() * omega.y},
cTransposeX{problem.c.transpose() * omega.x},
bTranspoesY{problem.b.transpose() * omega.y},
primal{getPrimal(problem, omega, psi)},
dual{getDual(problem, omega)},
primalDualGap{getGap(problem, omega)},
unbounded{cTransposeX < 0 ? AXPlusS.norm() * normOfC / -cTransposeX
: NAN},
infeasible{bTranspoesY < 0 ? ATransposeY.norm() * normOfB / -bTranspoesY
: NAN} {}
/**
* @function tryStepGoal
*/
M_RRT::StepResult M_RRT::tryStepGoal( const Eigen::VectorXd &qtry, int NNidx )
{
VectorXd qnear( ndim );
VectorXd qnew( ndim );
qnear = configVector[NNidx];
Eigen::VectorXd diff = ( qtry - qnear );
double edist = diff.norm();
// If the new node is nearer than the stepSize, don't add it
if( edist < stepSize )
{ return STEP_REACHED; }
// Scale it in order to add it
double scale = stepSize / edist;
for( int i = 0; i < ndim; i++ )
{ qnew[i] = qnear[i] + diff[i]*scale; }
double qnearCost = heuristicCost( qnear );
double qnewCost = heuristicCost( qnew );
//-- Check if there are not collisions are if the heuristic distance decreases
if( checkCollisions(qnew) )
{ return STEP_COLLISION; }
else if( qnewCost > qnearCost )
{ return STEP_NO_NEARER;}
else
{ addNode( qnew, NNidx );
return STEP_PROGRESS; }
}
void FindClosestQuad:: find
(
mesh::Quad& quad )
{
TRACE(quad.vertex(0).getCoords(), quad.vertex(1).getCoords(), quad.vertex(2).getCoords() , quad.vertex(3).getCoords());
// Methodology of book "Computational Geometry", Joseph O' Rourke, Chapter 7.2
auto& a = quad.vertex(0).getCoords();
auto& b = quad.vertex(1).getCoords();
auto& c = quad.vertex(2).getCoords();
auto& d = quad.vertex(3).getCoords();
auto& norm = quad.getNormal();
auto ret = math::barycenter::calcBarycentricCoordsForQuad(a, b, c, d, norm, _searchPoint);
assertion(ret.barycentricCoords.size() == 4);
const bool inside = not (ret.barycentricCoords.array() < - math::NUMERICAL_ZERO_DIFFERENCE).any();
// if valid, compute distance to triangle and evtl. store distance
if (inside) {
Eigen::VectorXd distanceVector = ret.projected;
distanceVector -= _searchPoint;
double distance = distanceVector.norm();
if ( _shortestDistance > distance ) {
_shortestDistance = distance;
_vectorToProjectionPoint = distanceVector;
_parametersProjectionPoint[0] = ret.barycentricCoords[0];
_parametersProjectionPoint[1] = ret.barycentricCoords[1];
_parametersProjectionPoint[2] = ret.barycentricCoords[2];
_parametersProjectionPoint[3] = ret.barycentricCoords[3];
_closestQuad = &quad;
}
}
}
/**
* @function tryStep
*/
JG_RRT::StepResult JG_RRT::tryStep( const Eigen::VectorXd &qtry, int NNidx )
{
Eigen::VectorXd qnear( ndim );
Eigen::VectorXd qnew( ndim );
qnear = configVector[NNidx];
Eigen::VectorXd diff = ( qtry - qnear );
double edist = diff.norm();
// If the new node is nearer than the stepSize, don't add it
if( edist < stepSize )
{
return STEP_REACHED;
}
// Scale it in order to add it
double scale = stepSize / edist;
for( int i = 0; i < ndim; i++ )
{
qnew[i] = qnear[i] + diff[i]*scale;
}
if( !checkCollisions(qnew) )
{
addNode( qnew, NNidx );
return STEP_PROGRESS;
}
else
{
return STEP_COLLISION;
}
}
void dampnewton(const FuncType &F, const JacType &DF,
Eigen::VectorXd &x, double rtol = 1e-4,double atol = 1e-6)
{
const int n = x.size();
const double lmin = 1E-3; // Minimal damping factor
double lambda = 1.0; // Initial and actual damping factor
Eigen::VectorXd s(n),st(n); // Newton corrections
Eigen::VectorXd xn(n); // Tentative new iterate
double sn,stn; // Norms of Newton corrections
do {
auto jacfac = DF(x).lu(); // LU-factorize Jacobian
s = jacfac.solve(F(x)); // Newton correction
sn = s.norm(); // Norm of Newton correction
lambda *= 2.0;
do {
lambda /= 2;
if (lambda < lmin) throw "No convergence: lambda -> 0";
xn = x-lambda*s; // {\bf Tentative next iterate}
st = jacfac.solve(F(xn)); // Simplified Newton correction
stn = st.norm();
}
while (stn > (1-lambda/2)*sn); // {\bf Natural monotonicity test}
x = xn; // Now: xn accepted as new iterate
lambda = std::min(2.0*lambda,1.0); // Try to mitigate damping
}
// Termination based on simplified Newton correction
while ((stn > rtol*x.norm()) && (stn > atol));
}
/* ********************************************************************************************* */
Vector Factors::RestPlate::errorProxy(const LieVector& a0, const LieVector& a1,
const LieVector& b0, const LieVector& b1) const {
// Get the two lines
Eigen::VectorXd a0v = a0.vector(), a1v = a1.vector(), b0v = b0.vector(), b1v = b1.vector();
Eigen::VectorXd aLine = a1v - a0v, bLine = b1v - b0v;
double aLength = aLine.norm(), bLength = bLine.norm();
aLine = aLine / aLength, bLine = bLine / bLength;
Eigen::Vector2d aPerp(-aLine(1), aLine(0)), bPerp(-bLine(1), bLine(0));
// Get the two projections (perpendicular projection should be 0.0).
Eigen::VectorXd va = b0v - a0v, vb = a1v - b0v;
double aParaProj = aLine(0) * va(0) + aLine(1) * va(1);
double bParaProj = bLine(0) * vb(0) + bLine(1) * vb(1);
double aPerpError = aPerp(0) * va(0) + aPerp(1) * va(1);
double bPerpError = bPerp(0) * vb(0) + bPerp(1) * vb(1);
// Get the parallel errors
double aParaError = 0.0, bParaError = 0.0;
if(aParaProj > aLength) aParaError = aParaProj - aLength;
else if(aParaProj < 0.0) aParaError = aParaProj;
if(bParaProj > bLength / 2) bParaError = bParaProj - bLength / 2;
else if(bParaProj < 0.0) bParaError = bParaProj;
// Send the minimal error
if(fabs(aPerpError) < fabs(bPerpError)) return Vector_(2, aPerpError, aParaError);
else return Vector_(2, bPerpError, bParaError);
}
void randomUnitSphere(Eigen::VectorXd &sp) {
static boost::random::mt19937 rng(time(NULL));
static boost::random::normal_distribution<> normal;
do {
sp(0) = normal(rng);
sp(1) = normal(rng);
sp(2) = normal(rng);
} while (sp.norm() < 0.00001);
sp.normalize();
}
/* ********************************************************************************************* */
Vector Factors::Alignment::errorProxy(const LieVector& p1, const LieVector& p2, const LieVector& p3)
const {
static const double offset = 0.0;
// 1- Compute the error along the line - the projection of the point to the [p1,p2] line
// should be between p1 and p2.
Eigen::VectorXd p1v = p1.vector();
Eigen::VectorXd p2v = p2.vector();
Eigen::VectorXd p3v = p3.vector();
p3v(0) += vert(0);
p3v(1) += vert(1);
// Get the vector from p1 to p2
Eigen::VectorXd vLine = p2v - p1v;
double length = vLine.norm();
assert(length != 0.0);
vLine = vLine / length;
// Get the distance between p1 and projected p3
// p3Offset here
Eigen::VectorXd v3 = (p3v) - p1v;
double p3Distance = vLine(0) * v3(0) + vLine(1) * v3(1);
// Decide which case the first error is in: (1) p3 is after p2, (2) p3 is between [p1,p2] or
// (3) p3 is before p1 and set the error accordingly.
size_t caseID;
double error1;
if(p3Distance > (length - offset)) {
caseID = 0;
error1 = (length - offset) - p3Distance;
}
else if(p3Distance < offset) {
caseID = 2;
error1 = p3Distance - offset;
}
else {
caseID = 1;
error1 = 0.0;
}
// 2- Compute the error associated with the perpendicular distance of p3 from line [p1,p2]
// Get the perpendicular vector
Eigen::Vector2d vPerp(-vLine(1), vLine(0));
// The error is the projection of p3 to the perpendicular vector
double error2 = vPerp(0) * v3(0) + vPerp(1) * v3(1) + 0.01;
// 3- Return the error
return Vector_(2, error1, error2);
}
void randomUnitQuaternion(Eigen::VectorXd &quat) {
static boost::random::mt19937 rng(time(NULL));
static boost::random::normal_distribution<> normal;
do {
quat(0) = normal(rng);
quat(1) = normal(rng);
quat(2) = normal(rng);
quat(3) = normal(rng);
} while (quat.norm() < 0.00001);
quat.normalize();
}
// Convert difference in desired and sensed forces to cartesian velocity.
// Implemented as constant step (equal to force_tracking_gain) in the direction of force error.
// This works with the integration (bug) in the low level controller. It relies on the low level
// controller integrating the constant error until it reaches the desired force
void forceErrorToVelocity(const Eigen::VectorXd& force_err, Eigen::VectorXd& cart_vel) {
double force_err_nrm;
if(bOrientCtrl) {
force_err_nrm = force_err.norm();
} else {
double force_error = force_err[0]*force_err[0]+force_err[1]*force_err[1]+force_err[2]*force_err[2];
force_err_nrm = sqrt(force_error);
}
if(bOrientCtrl) {
for(int i=0; i<6; ++i) {
cart_vel[i] = force_err[i]/force_err_nrm*force_tracking_gain;
}
} else {
for(int i=0; i<3; ++i) {
cart_vel[i] = force_err[i]/force_err_nrm*force_tracking_gain;
}
}
}
/**
* @function tryStepFromNode
* @brief Tries to extend tree towards provided sample (must be overridden for MBP )
*/
B1RRT::StepResult B1RRT::tryStepFromNode( const Eigen::VectorXd &_qtry, int _NNIdx ) {
/// Calculates a new node to grow towards _qtry, check for collisions and adds to tree
Eigen::VectorXd qnear = configVector[_NNIdx];
/// Compute direction and magnitude
Eigen::VectorXd diff = _qtry - qnear;
double dist = diff.norm();
if( dist < stepSize ) {
return STEP_REACHED;
}
/// Scale this vector to stepSize and add to end of qnear
Eigen::VectorXd qnew = qnear + diff*(stepSize/dist);
if( !checkCollisions(qnew) ) {
addNode( qnew, _NNIdx );
return STEP_PROGRESS;
} else {
return STEP_COLLISION;
}
}
void omxSD(GradientOptimizerContext &rf)
{
int maxIter = rf.maxMajorIterations;
if (maxIter == -1) maxIter = 50000;
Eigen::VectorXd currEst(rf.numFree);
rf.copyToOptimizer(currEst.data());
int iter = 0;
double priorSpeed = 1.0, shrinkage = 0.7;
rf.setupSimpleBounds();
rf.informOut = INFORM_UNINITIALIZED;
{
int mode = 0;
rf.solFun(currEst.data(), &mode);
if (mode == -1) {
rf.informOut = INFORM_STARTING_VALUES_INFEASIBLE;
return;
}
}
double refFit = rf.getFit();
rf.grad.resize(rf.numFree);
fit_functional ff(rf);
Eigen::VectorXd majorEst = currEst;
while(++iter < maxIter && !isErrorRaised()) {
gradient_with_ref(rf.gradientAlgo, 1, rf.gradientIterations, rf.gradientStepSize,
ff, refFit, majorEst, rf.grad);
if (rf.verbose >= 3) mxPrintMat("grad", rf.grad);
if(rf.grad.norm() == 0)
{
rf.informOut = INFORM_CONVERGED_OPTIMUM;
if(rf.verbose >= 2) mxLog("After %i iterations, gradient achieves zero!", iter);
break;
}
int retries = 300;
double speed = std::min(priorSpeed, 1.0);
double bestSpeed = speed;
bool foundBetter = false;
Eigen::VectorXd bestEst(majorEst.size());
Eigen::VectorXd prevEst(majorEst.size());
Eigen::VectorXd searchDir = rf.grad;
searchDir /= searchDir.norm();
prevEst.setConstant(nan("uninit"));
while (--retries > 0 && !isErrorRaised()){
Eigen::VectorXd nextEst = majorEst - speed * searchDir;
nextEst = nextEst.cwiseMax(rf.solLB).cwiseMin(rf.solUB);
if (nextEst == prevEst) break;
prevEst = nextEst;
rf.checkActiveBoxConstraints(nextEst);
int mode = 0;
double fit = rf.solFun(nextEst.data(), &mode);
if (fit < refFit) {
foundBetter = true;
refFit = rf.getFit();
bestSpeed = speed;
bestEst = nextEst;
break;
}
speed *= shrinkage;
}
if (false && foundBetter) {
// In some tests, this did not help so it is not enabled.
// It might be worth testing more.
mxLog("trying larger step size");
retries = 3;
while (--retries > 0 && !isErrorRaised()){
speed *= 1.01;
Eigen::VectorXd nextEst = majorEst - speed * searchDir;
nextEst = nextEst.cwiseMax(rf.solLB).cwiseMin(rf.solUB);
rf.checkActiveBoxConstraints(nextEst);
int mode = 0;
double fit = rf.solFun(nextEst.data(), &mode);
if (fit < refFit) {
foundBetter = true;
refFit = rf.getFit();
bestSpeed = speed;
bestEst = nextEst;
}
}
}
if (!foundBetter) {
rf.informOut = INFORM_CONVERGED_OPTIMUM;
if(rf.verbose >= 2) mxLog("After %i iterations, cannot find better estimation along the gradient direction", iter);
break;
}
if (rf.verbose >= 2) mxLog("major fit %f bestSpeed %g", refFit, bestSpeed);
majorEst = bestEst;
priorSpeed = bestSpeed * 1.1;
}
rf.est = majorEst;
if ((rf.grad.array().abs() > 0.1).any()) {
rf.informOut = INFORM_NOT_AT_OPTIMUM;
}
if (iter >= maxIter - 1) {
rf.informOut = INFORM_ITERATION_LIMIT;
if(rf.verbose >= 2) mxLog("Maximum iteration achieved!");
}
if(rf.verbose >= 1) mxLog("Status code : %i", rf.informOut);
}
void calculateOverlappingFOV(const cameras::CameraGeometryBase & camera1,
const cameras::CameraGeometryBase & camera2,
const sm::kinematics::Transformation& T_cam1_cam2,
cameras::ImageMask& outMask1,
cameras::ImageMask& outMask2,
double& outOverlappingRatio1,
double& outOverlappingRatio2,
const Eigen::VectorXi& sampleDistances,
double scale) {
// get dimensions
int width1 = (int) camera1.width() * scale;
int height1 = (int) camera1.height() * scale;
int width2 = (int) camera2.width() * scale;
int height2 = (int) camera2.height() * scale;
// clear incoming masks
cv::Mat outMask1CV = outMask1.getMask();
cv::Mat outMask2CV = outMask2.getMask();
outMask1CV = cv::Mat::zeros(height1, width1, CV_8UC1);
outMask2CV = cv::Mat::zeros(height2, width2, CV_8UC1);
outMask1.setScale(scale);
outMask2.setScale(scale);
Eigen::VectorXd point;
// build outMask1
outOverlappingRatio1 = 0;
for (int x = 0; x < width1; x++) {
for (int y = 0; y < height1; y++) {
// if visible
// TODO: BB: what about rounding mistakes?
Eigen::VectorXd kp(2);
kp << (double) x / scale, (double) y / scale;
if (camera1.vsKeypointToHomogeneous(kp, point)) {
double norm = point.norm();
int i = 0;
// check points on the beam until one is found or no other points to check
while (i < sampleDistances.size()
&& outMask1CV.at<unsigned char>(y, x) == 0) {
// Send point to distance sampleDistances(i) on the beam
Eigen::Vector4d tempPoint = point;
tempPoint(3) = norm / (norm + sampleDistances(i));
// Project point to other camera frame
tempPoint = T_cam1_cam2.inverse() * tempPoint;
Eigen::VectorXd keypointLocation;
if (camera2.vsHomogeneousToKeypoint(tempPoint, keypointLocation)) {
outMask1CV.at<unsigned char>(y, x) = 255;
outOverlappingRatio1++;
} // if
i++;
} // while
} // if
} // for
} // for
outOverlappingRatio1 = outOverlappingRatio1 / (width1 * height1);
// build outMask2
outOverlappingRatio2 = 0;
for (int x = 0; x < width2; x++) {
for (int y = 0; y < height2; y++) {
Eigen::VectorXd kp(2);
kp << (double) x / scale, (double) y / scale;
// if visible
// TODO: BB: what about rounding mistakes?
if (camera2.vsKeypointToHomogeneous(kp, point)) {
double norm = point.norm();
int i = 0;
// check points on the beam until one is found or no other points to check
while (i < sampleDistances.size()
&& outMask2CV.at<unsigned char>(y, x) == 0) {
// Send point to distance sampleDistances(i) on the beam
Eigen::Vector4d tempPoint = point;
tempPoint(3) = norm / (norm + sampleDistances(i));
// Project point to other camera frame
tempPoint = T_cam1_cam2 * tempPoint;
Eigen::VectorXd keypointLocation;
if (camera1.vsHomogeneousToKeypoint(tempPoint, keypointLocation)) {
outMask2CV.at<unsigned char>(y, x) = 255;
outOverlappingRatio2++;
} // if
i++;
} // while
} // if
} // for
} // for
outOverlappingRatio2 = outOverlappingRatio2 / (width2 * height2);
}
int main(int argc, char* argv[])
{
// Relaxation parameters
double omega_start = 0.95;
double omega_delta = 0.25;
int omega_count = 2;
// Tolerance factor
double tol=1e-6;
// Output precision
int p=9;
// Matrix dimension
int N=8;
// Band width
int band=3;
// Matrix specification
Eigen::ArrayXXd A = Eigen::ArrayXXd::Zero(N,2*band+1);
//diagonal
for (int i=0; i<N; i++) {A(i,band) = 8.0;}
//lower diagonal
for (int i=1; i<N; i++) {
//if (i>=2) A(i,band-2) = 3;
//if (i>=3) A(i,band-3) = 1;
A(i,band-2) = 2.0;
A(i,band-3) = 1.0;
}
//upper diagonal
for (int i=0; i<N-1; i++)
{
//if (i<=5) A(i,band+2) = -2;
//if (i<=4) A(i,band+3) = -1;
A(i,band+2) = -1.0;
A(i,band+3) = -2.0;
}
Eigen::MatrixXd b = Eigen::VectorXd::Zero(N);
for (int i=0; i<N; i++) {
b(i) = (2.0*i-3.0)/(2.0*i*i+1.0);
}
// Jacobi iterative solve
std::tuple<Eigen::VectorXd, int> res = jacobi(A, band, b, tol);
Eigen::VectorXd x = std::get<0>(res);
int ic = std::get<1>(res);
Eigen::VectorXd r = b - band_mult(A,band,band,x);
std::cout << std::fixed
<< std::setprecision(p)
<< "Jacobi: ,"
<< ", Iterations =, " << ic
<< ", residual =, " << r.norm()
<< std::endl;
std::cout << "x = " << std::endl;
for (int i=0; i<N; i++) {
std::cout << std::fixed << std::setprecision(p) << x(i) << std::endl;
}
// Gauss-Seidel iterative solve
res = gs(A, band, b, tol);
x = std::get<0>(res);
ic = std::get<1>(res);
r = b - band_mult(A,band,band,x);
std::cout << std::fixed
<< std::setprecision(p)
<< "Gauss-Seidel: ,"
<< ", Iterations =, " << ic
<< ", residual =, " << r.norm()
<< std::endl;
std::cout << "x = " << std::endl;
for (int i=0; i<N; i++) {
std::cout << std::fixed << std::setprecision(p) << x(i) << std::endl;
}
// SOR iterative solve for all omega values
double omega = omega_start;
for (int k=0; k<omega_count; k++) {
std::tuple<Eigen::VectorXd, int> res = sor(omega, A, band, b, tol);
Eigen::VectorXd x = std::get<0>(res);
int ic = std::get<1>(res);
Eigen::VectorXd r = b - band_mult(A,band,band,x);
std::cout << std::fixed
<< std::setprecision(p)
<< "Omega = ," << omega
<< ", Iterations =, " << ic
<< ", residual =, " << r.norm()
<< std::endl;
std::cout << "x = " << std::endl;
for (int i=0; i<N; i++) {
std::cout << std::fixed << std::setprecision(p) << x(i) << std::endl;
}
omega += omega_delta;
}
return 0;
}
int main() {
const int N = 10; // number of cameras
const int F = 0; // number of fixed cams
MotionMatrix<N> reconstruction; // camera matrices
ShapeMatrix shape; // 3D-points
ImageTransform<N> Ks,Rs; // camera matrices
MeasurementMatrix<N> correspondences; // correspondences
ImageTransform<F> fixedIntrinsics;
Affine2d K;
Matrix3d R;
// --- synthesise a vector of pixel correspondences
// ------------------------------------------------
Eigen::Matrix4d H;
Eigen::VectorXd err;
int i;
try {
correspondences.load("/home/daniel/data/data/correspondence-test");
reconstruction.svd_solve(correspondences);
std::cout << "SVD reconstructed camera matrix =" << std::endl;
std::cout << reconstruction << std::endl;
H = reconstruction.diac_euclidean_lift(5);
reconstruction *= H;
std::cout << "Projective transform =" << std::endl;
std::cout << H << std::endl;
reconstruction.reprojection_err(correspondences, err);
std::cout << "SVD reprojection err = " << std::endl;
std::cout << err.norm()/sqrt(err.rows())<<std::endl;
std::cout << "Reconstructed camera matrix =" << std::endl;
std::cout << reconstruction << std::endl;
reconstruction.KR_decompose(Ks,Rs);
std::cout << "Intrinsics are" << std::endl;
std::cout << Ks << std::endl;
std::cout << "Rotations are" << std::endl;
std::cout << Rs << std::endl;
}
catch(const char *err) {
std::cout << "Caught error: " << err << std::endl;
}
/******
reconstruction = cameras;
reconstruction += MotionMatrix<N>::Random()*0.001;
reconstruction.view(0) = cameras.view(0);
std::cout << "Starting Bundle Adjustment" << std::endl;
reconstruction.bundle_adjust(1, correspondences);
shape.solve(correspondences, reconstruction);
reconstruction.reprojection_err(correspondences, err);
std::cout << "Adjusted reprojection err = " << std::endl;
std::cout << err.norm()/(correspondences.cols()*N*2.0*(1.0-0.08))<<std::endl;
std::cout << "Adjusted camera matrix =" << std::endl;
std::cout << reconstruction << std::endl;
skew = reconstruction.KR_decompose(Ks,Rs);
std::cout << "Intrinsics are" << std::endl;
std::cout << Ks << std::endl;
std::cout << "Rotations are" << std::endl;
std::cout << Rs << std::endl;
*****/
return(0);
}
// Solves ||Ax - b||_1 for the optimal L1 solution given an initial guess for
// x. To solve this we introduce an auxiliary variable y such that the
// solution to:
// min 1 * y
// s.t. [ A -I ] [ x ] < [ b ]
// [ -A -I ] [ y ] [ -b ]
// which is an equivalent linear program.
bool Solve
(
const Eigen::VectorXd& rhs,
Eigen::VectorXd* solution
)
{
// Since constructor was called before we check Compute status
if(linear_solver_.info() != Eigen::Success)
{
std::cerr << "Cannot compute the matrix factorization" << std::endl;
return false;
}
Eigen::VectorXd& x = *solution;
Eigen::VectorXd z(a_.rows()), u(a_.rows());
z.setZero();
u.setZero();
Eigen::VectorXd a_times_x(a_.rows()), z_old(z.size()), ax_hat(a_.rows());
// Precompute some convergence terms.
const double rhs_norm = rhs.norm();
const double primal_abs_tolerance_eps =
std::sqrt(a_.rows()) * options_.absolute_tolerance;
const double dual_abs_tolerance_eps =
std::sqrt(a_.cols()) * options_.absolute_tolerance;
for (int i = 0; i < options_.max_num_iterations; ++i)
{
// Update x.
x.noalias() = linear_solver_.solve(a_.transpose() * (rhs + z - u));
a_times_x.noalias() = a_ * x;
ax_hat.noalias() = options_.alpha * a_times_x;
ax_hat.noalias() += (1.0 - options_.alpha) * (z + rhs);
// Update z and set z_old.
std::swap(z, z_old);
z.noalias() = Shrinkage(ax_hat - rhs + u, 1.0 / options_.rho);
// Update u.
u.noalias() += ax_hat - z - rhs;
// Compute the convergence terms.
const double r_norm = (a_times_x - z - rhs).norm();
const double s_norm =
(-options_.rho * a_.transpose() * (z - z_old)).norm();
const double max_norm =
std::max({a_times_x.norm(), z.norm(), rhs_norm});
const double primal_eps =
primal_abs_tolerance_eps + options_.relative_tolerance * max_norm;
const double dual_eps =
dual_abs_tolerance_eps +
options_.relative_tolerance *
(options_.rho * a_.transpose() * u).norm();
// Log the result to the screen.
// std::ostringstream os;
// os << "Iteration: " << i << "\n"
// << "R norm: " << r_norm << "\n"
// << "S norm: " << s_norm << "\n"
// << "Primal eps: " << primal_eps << "\n"
// << "Dual eps: " << dual_eps << std::endl;
// std::cout << os.str() << std::endl;
// Determine if the minimizer has converged.
if (r_norm < primal_eps && s_norm < dual_eps)
{
return true;
}
}
return false;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "cart_to_joint");
ros::NodeHandle nh;
ros::NodeHandle _nh("~");
if(!parseParams(_nh)) {
ROS_ERROR("Errors while parsing arguments.");
return 1;
}
// Initialize robot with/without orientation (variable from launch file)
mRobot = new RTKRobotArm(bOrientCtrl);
if(!mRobot->initialize(_nh)) {
ROS_ERROR("Error while loading robot");
return 1;
}
numdof = mRobot->numdof;
des_ee_ft.resize(6);
est_ee_ft.resize(6);
des_ee_stiff.resize(6);
shift_ee_ft.resize(6);
for(int i=0; i<6; ++i) {
des_ee_ft(i)=0;
est_ee_ft(i)=0;
shift_ee_ft(i)=0;
}
read_jpos.resize(numdof);
joint_lims.resize(numdof);
// TODO: need to find a better way to do this from launch files
/********** Specific for RTKRobotArm (KUKA) ****************/
/* joint_lims[0] = 150;joint_lims[1] = 107;joint_lims[2] = 150; joint_lims[3] = 107;
joint_lims[4] = 150;joint_lims[5] = 115;joint_lims[6] = 150;*/
/********** Specific for RTKRobotArm (BOXY) ****************/
joint_lims[0] = 150;joint_lims[1] = 107;joint_lims[2] = 150; joint_lims[3] = 107;
joint_lims[4] = 150;joint_lims[5] = 115;joint_lims[6] = 150;
joint_lims = joint_lims*M_PI/180.0;
mRobot->setJointLimits(joint_lims);
Eigen::VectorXd np; np.resize(numdof);
np[0] = -0.62; np[1] = 0.76; np[2] = 0.61; np[3] = -1.0;
np[4] = -0.40; np[5] = 1.4; np[6] = 1.4;
// TODO: not implemented properly yet
// mRobot->setNullPosture(np);
/*********************************************************/
if(bUseIAI) {
msg_vel_stiff_iai.stiffness.resize(numdof);
msg_vel_stiff_iai.damping.resize(numdof);
msg_vel_stiff_iai.add_torque.resize(numdof);
msg_vel_stiff_iai.velocity.resize(numdof);
} else {
msg_vel_stiff_jstate.velocity.resize(numdof);
}
if(bUseIAI) {
ROS_INFO_STREAM("Using iai_control_msgs");
pub_joints = nh.advertise<iai_control_msgs::MultiJointVelocityImpedanceCommand>(output_joints_topic, 3);
} else {
ROS_INFO_STREAM("Joints states");
// pub_joints = nh.advertise<sensor_msgs::JointState>(output_joints_topic, 3);
pub_joints = nh.advertise<kuka_fri_bridge::JointStateImpedance>(output_joints_topic, 3);
}
ros::Subscriber sub_pos = nh.subscribe<geometry_msgs::PoseStamped>(input_pose_topic, 1, cartCallback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_ft = nh.subscribe<geometry_msgs::WrenchStamped>(input_ft_topic, 1, ftCallback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_stiff = nh.subscribe<geometry_msgs::WrenchStamped>(input_stiff_topic, 1, stiffnessCallback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_est_ft = nh.subscribe<geometry_msgs::WrenchStamped>(input_estimate_ft_topic, 1, estFTCallback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_jstate = nh.subscribe<sensor_msgs::JointState>(input_joint_topic, 1, jointStateCallback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub = nh.subscribe("Action_State", 1, actionStateCallback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_ja = nh.subscribe<std_msgs::Bool>("joint_action", 1, jointActionCallback, ros::TransportHints().tcpNoDelay());
joint_vel.resize(numdof); joint_stiff.resize(numdof), joint_damp.resize(numdof);
for(int i=0; i<numdof; ++i) {
joint_vel[i] = 0.;
joint_stiff[i] = DEFAULT_JSTIFF;
joint_damp[i] = DEFAULT_JDAMP;
}
ROS_INFO("Waiting for robot joint state and FT estimate topic...");
ros::Rate r(1000);
isJointOkay = false; isFTOkay = false; isAllOkay = false;
while(ros::ok() && (!isJointOkay || (bUseForce && !isFTOkay)) ) {
// while(ros::ok()){
ros::spinOnce();
r.sleep();
}
tf::Pose tmp;
mRobot->setJoints(read_jpos);
mRobot->getEEPose(tmp);
des_ee_pose = tmp;
// Remove the residual force/torque from the future estimates. To compensate for error in ee force estimation or tool calibration
shift_ee_ft = est_ee_ft;
ROS_INFO_STREAM("Shift: "<<shift_ee_ft.transpose());
if(shift_ee_ft.norm() > ft_tol) {
ROS_WARN("Shift in EE FT estimate more than the required tolerance!!");
ROS_WARN("Either increase the tolerance or calibrate the tool better");
ros::Duration warn(2);
warn.sleep();
}
ROS_INFO("Node started");
isAllOkay = true;
// This is to ensure that we send atleast one zero velocity message before dying!
// For safety.
ros::Rate r_(1000);
while(ros::ok()) {
ros::spinOnce();
r_.sleep();
}
for(int i=0; i<numdof; ++i) {
joint_vel[i] = 0.0;
}
sendJointMessage(joint_vel, joint_stiff, joint_damp);
ros::Duration(1.0).sleep();
nh.shutdown();
return 0;
}
// Convert current cartesian commands to joint velocity
void computeJointVelocity(Eigen::VectorXd& jvel) {
if(jvel.size() != numdof) {
jvel.resize(numdof);
}
if(est_ee_ft.norm() > max_ee_ft_norm) {
for(int i=0; i<jvel.size(); ++i) {
jvel[i]=0.0;
}
ROS_WARN_THROTTLE(0.1, "Too much force! Sending zero velocity");
return;
}
// tf::Pose curr_ee_pose;
mRobot->setJoints(read_jpos);
mRobot->getEEPose(curr_ee_pose);
// Two kinds of cartesian velocities - due to position error and force error
Eigen::VectorXd vel_due_to_pos, vel_due_to_force;
if(bOrientCtrl) {
vel_due_to_pos.resize(6);
vel_due_to_force.resize(6);
} else {
vel_due_to_pos.resize(3);
vel_due_to_force.resize(3);
}
for(int i=0; i<vel_due_to_force.size(); ++i) {
vel_due_to_force[i] = 0;
vel_due_to_pos[i] = 0;
}
ROS_INFO_STREAM_THROTTLE(0.5, "Publishing EE TF");
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(des_ee_pose, ros::Time::now(), world_frame, "/des_ee_tf"));
br.sendTransform(tf::StampedTransform(curr_ee_pose, ros::Time::now(), world_frame, "/curr_ee_tf"));
// Cartesian velocity due to position/orientation error
tf::Vector3 linvel = des_ee_pose.getOrigin() - curr_ee_pose.getOrigin();
vel_due_to_pos(0) = linvel.getX();
vel_due_to_pos(1) = linvel.getY();
vel_due_to_pos(2) = linvel.getZ();
double pos_err = linvel.length();
ROS_INFO_STREAM_THROTTLE(0.5, "Position Err:\t"<< pos_err);
// Nadia's way...
// If Orientation is activated from launch file
double qdiff = acos(abs(des_ee_pose.getRotation().dot(curr_ee_pose.getRotation())));
if(bOrientCtrl) {
// Computing angular velocity using quaternion difference
tf::Quaternion tmp = curr_ee_pose.getRotation();
tf::Quaternion angvel = (des_ee_pose.getRotation() - tmp)*tmp.inverse();
vel_due_to_pos(3) = angvel.getX();
vel_due_to_pos(4) = angvel.getY();
vel_due_to_pos(5) = angvel.getZ();
ROS_INFO_STREAM_THROTTLE(0.5, "Orient. Err:\t"<<qdiff);
}
if (pos_err < 0.01 && qdiff < 0.05){ // LASA 0.5deg
// if (pos_err < 0.015 && qdiff < 0.05){ // Boxy 1.7 deg
vel_due_to_pos(3) = 0;
vel_due_to_pos(4) = 0;
vel_due_to_pos(5) = 0;
}
/*
// Aswhini's way...
// If Orientation is activated from launch file
if(bOrientCtrl) {
// Computing angular velocity using quaternion difference
tf::Quaternion tmp = curr_ee_pose.getRotation();
tf::Quaternion angvel = (des_ee_pose.getRotation() - tmp)*tmp.inverse();
vel_due_to_pos(3) = angvel.getX();
vel_due_to_pos(4) = angvel.getY();
vel_due_to_pos(5) = angvel.getZ();
tf::Quaternion t = angvel;
// ROS_INFO_STREAM_THROTTLE(0.5, "Orient. Err:\t"<<angvel.length());
}
*/
// Compute errors in position
double err = vel_due_to_pos.norm();
// Increase tracking performance by tuning traj_traking_gain
vel_due_to_pos *= traj_tracking_gain;
// If force is activated from launch file
if(bUseForce) {
// Compute force error
Eigen::VectorXd ft_err = des_ee_ft - est_ee_ft;
double ft_err_nrm;
if(bOrientCtrl) {
ft_err_nrm = ft_err.norm();
} else {
double force_error = ft_err[0]*ft_err[0] +ft_err[1]*ft_err[1]+ft_err[2]*ft_err[2];
ft_err_nrm = sqrt(force_error);
}
// Remove force if too small. This is necessary due to residual errors in EE force estimation.
// Dont apply external forces until desired position is almost reached within reach_tol
if(ft_err_nrm > ft_tol && err < reach_tol) {
forceErrorToVelocity(ft_err, vel_due_to_force);
}
ROS_INFO_STREAM_THROTTLE(0.5, "Force Err:\t"<<ft_err_nrm);
}
// Add the cartesian velocities due to position and force errors and compute the joint_velocity
// mRobot->getIKJointVelocity(vel_due_to_force+vel_due_to_pos, jvel);
mRobot->getIKJointVelocity(vel_due_to_pos, jvel);
}
void MohrCoulomb<DisplacementDim>::computeConstitutiveRelation(
double const t,
ProcessLib::SpatialPosition const& x,
double const aperture0,
Eigen::Ref<Eigen::VectorXd const>
sigma0,
Eigen::Ref<Eigen::VectorXd const>
w_prev,
Eigen::Ref<Eigen::VectorXd const>
w,
Eigen::Ref<Eigen::VectorXd const>
sigma_prev,
Eigen::Ref<Eigen::VectorXd>
sigma,
Eigen::Ref<Eigen::MatrixXd>
Kep,
typename FractureModelBase<DisplacementDim>::MaterialStateVariables&
material_state_variables)
{
material_state_variables.reset();
MaterialPropertyValues const mat(_mp, t, x);
Eigen::VectorXd const dw = w - w_prev;
const int index_ns = DisplacementDim - 1;
double const aperture = w[index_ns] + aperture0;
double const aperture_prev = w_prev[index_ns] + aperture0;
Eigen::MatrixXd Ke;
{ // Elastic tangent stiffness
Ke = Eigen::MatrixXd::Zero(DisplacementDim, DisplacementDim);
for (int i = 0; i < index_ns; i++)
Ke(i, i) = mat.Ks;
Ke(index_ns, index_ns) =
mat.Kn *
logPenaltyDerivative(aperture0, aperture, _penalty_aperture_cutoff);
}
Eigen::MatrixXd Ke_prev;
{ // Elastic tangent stiffness at w_prev
Ke_prev = Eigen::MatrixXd::Zero(DisplacementDim, DisplacementDim);
for (int i = 0; i < index_ns; i++)
Ke_prev(i, i) = mat.Ks;
Ke_prev(index_ns, index_ns) =
mat.Kn * logPenaltyDerivative(
aperture0, aperture_prev, _penalty_aperture_cutoff);
}
// Total plastic aperture compression
// NOTE: Initial condition sigma0 seems to be associated with an initial
// condition of the w0 = 0. Therefore the initial state is not associated
// with a plastic aperture change.
Eigen::VectorXd const w_p_prev =
w_prev - Ke_prev.fullPivLu().solve(sigma_prev - sigma0);
{ // Exact elastic predictor
sigma.noalias() = Ke * (w - w_p_prev);
sigma.coeffRef(index_ns) =
mat.Kn * w[index_ns] *
logPenalty(aperture0, aperture, _penalty_aperture_cutoff);
}
sigma.noalias() += sigma0;
double const sigma_n = sigma[index_ns];
// correction for an opening fracture
if (_tension_cutoff && sigma_n > 0)
{
Kep.setZero();
sigma.setZero();
material_state_variables.setTensileStress(true);
return;
}
// check shear yield function (Fs)
Eigen::VectorXd const sigma_s = sigma.head(DisplacementDim-1);
double const mag_tau = sigma_s.norm(); // magnitude
double const Fs = mag_tau + sigma_n * std::tan(mat.phi) - mat.c;
material_state_variables.setShearYieldFunctionValue(Fs);
if (Fs < .0)
{
Kep = Ke;
return;
}
Eigen::VectorXd dFs_dS(DisplacementDim);
dFs_dS.head(DisplacementDim-1).noalias() = sigma_s.normalized();
dFs_dS[index_ns] = std::tan(mat.phi);
// plastic potential function: Qs = |tau| + Sn * tan da
Eigen::VectorXd dQs_dS = dFs_dS;
dQs_dS[index_ns] = std::tan(mat.psi);
// plastic multiplier
Eigen::RowVectorXd const A = dFs_dS.transpose() * Ke / (dFs_dS.transpose() * Ke * dQs_dS);
double const d_eta = A * dw;
// plastic part of the dispalcement
Eigen::VectorXd const dwp = dQs_dS * d_eta;
// correct stress
sigma.noalias() = sigma_prev + Ke * (dw - dwp);
// Kep
Kep = Ke - Ke * dQs_dS * A;
}
/**
* Receives images as well the optical flow to calculate the focus of expansion
*/
void DirectionOfMovement::nextContainer(odcore::data::Container &a_c)
{
if(a_c.getDataType() == odcore::data::image::SharedImage::ID()){
odcore::data::image::SharedImage mySharedImg =
a_c.getData<odcore::data::image::SharedImage>();
// std::cout<<mySharedImg.getName()<<std::endl;
std::shared_ptr<odcore::wrapper::SharedMemory> sharedMem(
odcore::wrapper::SharedMemoryFactory::attachToSharedMemory(
mySharedImg.getName()));
const uint32_t nrChannels = mySharedImg.getBytesPerPixel();
const uint32_t imgWidth = mySharedImg.getWidth();
const uint32_t imgHeight = mySharedImg.getHeight();
IplImage* myIplImage = cvCreateImage(cvSize(imgWidth,imgHeight), IPL_DEPTH_8U,
nrChannels);
cv::Mat tmpImage = cv::Mat(myIplImage);
if(!sharedMem->isValid()){
return;
}
sharedMem->lock();
{
memcpy(tmpImage.data, sharedMem->getSharedMemory(),
imgWidth*imgHeight*nrChannels);
}
sharedMem->unlock();
m_image.release();
m_image = tmpImage.clone();
cvReleaseImage(&myIplImage);
return;
}
if(a_c.getDataType() == opendlv::sensation::OpticalFlow::ID()){
opendlv::sensation::OpticalFlow message =
a_c.getData<opendlv::sensation::OpticalFlow>();
uint16_t nPoints = message.getNumberOfPoints();
std::vector<opendlv::model::Direction> directions =
message.getListOfDirections();
std::vector<float> u = message.getListOfU();
std::vector<float> v = message.getListOfV();
Eigen::MatrixXd flow(nPoints, 4);
Eigen::MatrixXd A(nPoints,2);
Eigen::MatrixXd B(nPoints,1);
for(uint8_t i = 0; i < nPoints; ++i){
flow.row(i) << directions[i].getAzimuth(), directions[i].getZenith(),
u[i], v[i];
}
A.col(0) = flow.col(3);
A.col(1) = -flow.col(2);
B.col(0) = flow.col(3).cwiseProduct(flow.col(0))-flow.col(1).cwiseProduct(flow.col(2));
// FOE = (A * A^T)^-1 * A^T * B
Eigen::VectorXd FoeMomentum = ((A.transpose()*A).inverse() * A.transpose() * B) - m_foePix;
if(FoeMomentum.allFinite()){
if(FoeMomentum.norm() > 10){
m_foePix = m_foePix + FoeMomentum/FoeMomentum.norm()*10;
}
else{
m_foePix = m_foePix + FoeMomentum/20;
}
}
// SendContainer();
std::cout<< m_foePix.transpose() << std::endl;
cv::circle(m_image, cv::Point2f(m_foePix(0),m_foePix(1)), 3, cv::Scalar(0,0,255), -1, 8);
const int32_t windowWidth = 640;
const int32_t windowHeight = 480;
cv::Mat display;
cv::resize(m_image, display, cv::Size(windowWidth, windowHeight), 0, 0,
cv::INTER_CUBIC);
cv::imshow("FOE", display);
cv::waitKey(1);
return;
}
}