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();
}
void X3DConverter::startShape(const std::array<float, 12>& matrix) {
// Finding axis/angle from matrix using Eigen for its bullet proof implementation.
Eigen::Transform<float, 3, Eigen::Affine> t;
t.setIdentity();
for (unsigned int i = 0; i < 3; i++) {
for (unsigned int j = 0; j < 4; j++) {
t(i, j) = matrix[i+j*3];
}
}
Eigen::Matrix3f rotationMatrix;
Eigen::Matrix3f scaleMatrix;
t.computeRotationScaling(&rotationMatrix, &scaleMatrix);
Eigen::Quaternionf q;
Eigen::AngleAxisf aa;
q = rotationMatrix;
aa = q;
Eigen::Vector3f scale = scaleMatrix.diagonal();
Eigen::Vector3f translation = t.translation();
startNode(ID::Transform);
m_writers.back()->setSFVec3f(ID::translation, translation.x(), translation.y() , translation.z());
m_writers.back()->setSFRotation(ID::rotation, aa.axis().x(), aa.axis().y(), aa.axis().z(), aa.angle());
m_writers.back()->setSFVec3f(ID::scale, scale.x(), scale.y(), scale.z());
startNode(ID::Shape);
startNode(ID::Appearance);
startNode(ID::Material);
m_writers.back()->setSFColor<vector<float> >(ID::diffuseColor, RVMColorHelper::color(m_materials.back()));
endNode(ID::Material); // Material
endNode(ID::Appearance); // Appearance
}
/**
* @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;
}
Eigen::Matrix<Scalar, HOMOGENEOUS_TRANSFORM_SIZE, DerivedDT::ColsAtCompileTime> dHomogTransInv(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedDT>& dT) {
const int nq_at_compile_time = DerivedDT::ColsAtCompileTime;
int nq = dT.cols();
const auto& R = T.linear();
const auto& p = T.translation();
std::array<int, 3> rows {0, 1, 2};
std::array<int, 3> R_cols {0, 1, 2};
std::array<int, 1> p_cols {3};
auto dR = getSubMatrixGradient(dT, rows, R_cols, T.Rows);
auto dp = getSubMatrixGradient(dT, rows, p_cols, T.Rows);
auto dinvT_R = transposeGrad(dR, R.rows());
auto dinvT_p = (-R.transpose() * dp - matGradMult(dinvT_R, p)).eval();
const int numel = HOMOGENEOUS_TRANSFORM_SIZE;
Eigen::Matrix<Scalar, numel, nq_at_compile_time> ret(numel, nq);
setSubMatrixGradient(ret, dinvT_R, rows, R_cols, T.Rows);
setSubMatrixGradient(ret, dinvT_p, rows, p_cols, T.Rows);
// zero out gradient of elements in last row:
const int last_row = 3;
for (int col = 0; col < T.HDim; col++) {
ret.row(last_row + col * T.Rows).setZero();
}
return ret;
}
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
}
operator Eigen::Transform<double, 3, Eigen::Affine, _Options>() const
{
Eigen::Transform<double, 3, Eigen::Affine, _Options> ret;
ret.setIdentity();
ret.rotate(this->orientation);
ret.translation() = this->position;
return ret;
}
/**
* @function wsDiff
*/
Eigen::VectorXd JG_RRT::wsDiff( Eigen::VectorXd q )
{
Eigen::Transform<double, 3, Eigen::Affine> T;
robinaLeftArm_fk( q, TWBase, Tee, T );
Eigen::VectorXd ws_diff = ( goalPosition - T.translation() );
return ws_diff;
}
/**
* @function wsDistance
*/
double goWSOrient::wsDistance( Eigen::VectorXd q )
{
Eigen::Transform<double, 3, Eigen::Affine> T;
robinaLeftArm_fk( q, TWBase, Tee, T );
Eigen::VectorXd ws_diff = ( goalPos - T.translation() );
double ws_dist = ws_diff.norm();
return ws_dist;
}
/**
* @function plan
* @brief
*/
bool JG_RRT::plan( const Eigen::VectorXd &_startConfig,
const Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
const std::vector< Eigen::VectorXd > &_guidingNodes,
std::vector<Eigen::VectorXd> &path )
{
/** Store information */
this->startConfig = _startConfig;
this->goalPose = _goalPose;
this->goalPosition = _goalPose.translation();
//-- Initialize the search tree
addNode( startConfig, -1 );
//-- Add the guiding nodes
addGuidingNodes( _guidingNodes );
double p;
while( goalDistVector[activeNode] > distanceThresh )
{
//-- Generate the probability
p = RANDNM( 0.0, 1.0 );
//-- Apply either extension to goal (J) or random connection
if( p < p_goal )
{
if( extendGoal() == true )
{
break;
}
}
else
{
tryStep(); /*extendRandom();*/
}
// Check if we are still inside
if( configVector.size() > maxNodes )
{ cout<<"-- Exceeded "<<maxNodes<<" allowed - ws_dist: "<<goalDistVector[rankingVector[0]]<<endl;
break;
}
}
//-- If a path is found
if( goalDistVector[activeNode] < distanceThresh )
{ tracePath( activeNode, path );
cout<<"JG - Got a path! - nodes: "<<path.size()<<" tree size: "<<configVector.size()<<endl;
return true;
}
else
{ cout<<"--(!) JG :No successful path found! "<<endl;
return false;
}
}
/**
* @function Basic_M_RRT
* @brief
*/
bool M_RRT::BasicPlan( std::vector<Eigen::VectorXd> &path,
Eigen::VectorXd &_startConfig,
Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
Eigen::Transform<double, 3, Eigen::Affine> _TWBase,
Eigen::Transform<double, 3, Eigen::Affine> _Tee )
{
printf("Basic Plan \n");
/** Store information */
this->startConfig = _startConfig;
this->goalPose = _goalPose;
this->goalPosition = _goalPose.translation();
this->TWBase = _TWBase;
this->Tee = _Tee;
//-- Initialize the search tree
addNode( startConfig, -1 );
//-- Calculate the heuristicThreshold
heuristicThresh = w1*distThresh + w2*abs( cos( xAlignThresh ) - 1 ) +w3*abs( cos( yAlignThresh ) );
//-- Let's start the loop
double p;
double heuristic = heuristicVector[0];
int gc = 0; int rc = 0;
while( heuristic > heuristicThresh )
{
//-- Probability
p = rand()%100;
//-- Either extends towards goal or random
if( p < pGoal )
{ printf("Goal \n");extendGoal(); gc++; }
else
{ printf("Random \n"); extendRandom(); rc++;}
//-- If bigger than maxNodes, get out loop
if( maxNodes > 0 && configVector.size() > maxNodes )
{
cout<<"** Exceeded "<<maxNodes<<" MinCost: "<<heuristicVector[minHeuristicIndex()]<<"MinRankingCost: "<<heuristicVector[rankingVector[0]]<<endl;
printf("Goal counter: %d, random counter: %d \n", gc, rc );
return false; }
heuristic = heuristicVector[ rankingVector[0] ];
}
printf("Goal counter: %d, random counter: %d \n", gc, rc );
printf( "-- Plan successfully generated with %d nodes \n", configVector.size() );
tracePath( activeNode, path );
return true;
}
/**
* @function workspaceDist
* @brief
*/
Eigen::VectorXd M_RRT::workspaceDist( Eigen::VectorXd node, Eigen::VectorXd ws_target )
{
Eigen::Transform<double, 3, Eigen::Affine> T;
Eigen::VectorXd diff;
// Calculate the EE position
robinaLeftArm_fk( node, TWBase, Tee, T );
Eigen::VectorXd ws_node = T.translation();
// Calculate the workspace distance to goal
diff = ( ws_target - ws_node );
return diff;
}
typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type dTransformAdjointTranspose(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedX>& X,
const Eigen::MatrixBase<DerivedDT>& dT,
const Eigen::MatrixBase<DerivedDX>& dX) {
assert(dT.cols() == dX.cols());
int nq = dT.cols();
const auto& R = T.linear();
const auto& p = T.translation();
std::array<int, 3> rows {0, 1, 2};
std::array<int, 3> R_cols {0, 1, 2};
std::array<int, 1> p_cols {3};
auto dR = getSubMatrixGradient(dT, rows, R_cols, T.Rows);
auto dp = getSubMatrixGradient(dT, rows, p_cols, T.Rows);
auto Rtranspose = R.transpose();
auto dRtranspose = transposeGrad(dR, R.rows());
typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type ret(X.size(), nq);
std::array<int, 3> Xomega_rows {0, 1, 2};
std::array<int, 3> Xv_rows {3, 4, 5};
for (int col = 0; col < X.cols(); col++) {
auto Xomega_col = X.template block<3, 1>(0, col);
auto Xv_col = X.template block<3, 1>(3, col);
std::array<int, 1> col_array {col};
auto dXomega_col = getSubMatrixGradient(dX, Xomega_rows, col_array, X.rows());
auto dXv_col = getSubMatrixGradient(dX, Xv_rows, col_array, X.rows());
auto dp_hatXv_col = (dp.colwise().cross(Xv_col) - dXv_col.colwise().cross(p)).eval();
auto Xomega_col_minus_p_cross_Xv_col = (Xomega_col - p.cross(Xv_col)).eval();
auto dXomega_transformed_col = (Rtranspose * (dXomega_col - dp_hatXv_col) + matGradMult(dRtranspose, Xomega_col_minus_p_cross_Xv_col)).eval();
auto dRtransposeXv_col = (Rtranspose * dXv_col + matGradMult(dRtranspose, Xv_col)).eval();
setSubMatrixGradient(ret, dXomega_transformed_col, Xomega_rows, col_array, X.rows());
setSubMatrixGradient(ret, dRtransposeXv_col, Xv_rows, col_array, X.rows());
}
return ret;
}
CalibrateKinectCheckerboard()
: nh_("~"), it_(nh_), calibrated(false)
{
// Load parameters from the server.
nh_.param<std::string>("fixed_frame", fixed_frame, "/base_link");
nh_.param<std::string>("camera_frame", camera_frame, "/camera_link");
nh_.param<std::string>("target_frame", target_frame, "/calibration_pattern");
nh_.param<std::string>("tip_frame", tip_frame, "/gripper_link");
nh_.param<int>("checkerboard_width", checkerboard_width, 6);
nh_.param<int>("checkerboard_height", checkerboard_height, 7);
nh_.param<double>("checkerboard_grid", checkerboard_grid, 0.027);
// Set pattern detector sizes
pattern_detector_.setPattern(cv::Size(checkerboard_width, checkerboard_height), checkerboard_grid, CHESSBOARD);
transform_.translation().setZero();
transform_.matrix().topLeftCorner<3, 3>() = Quaternionf().setIdentity().toRotationMatrix();
// Create subscriptions
info_sub_ = nh_.subscribe("/camera/rgb/camera_info", 1, &CalibrateKinectCheckerboard::infoCallback, this);
// Also publishers
pub_ = it_.advertise("calibration_pattern_out", 1);
detector_pub_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZ> >("detector_cloud", 1);
physical_pub_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZ> >("physical_points_cloud", 1);
// Create ideal points
ideal_points_.push_back( pcl::PointXYZ(0, 0, 0) );
ideal_points_.push_back( pcl::PointXYZ((checkerboard_width-1)*checkerboard_grid, 0, 0) );
ideal_points_.push_back( pcl::PointXYZ(0, (checkerboard_height-1)*checkerboard_grid, 0) );
ideal_points_.push_back( pcl::PointXYZ((checkerboard_width-1)*checkerboard_grid, (checkerboard_height-1)*checkerboard_grid, 0) );
// Create proper gripper tip point
nh_.param<double>("gripper_tip_x", gripper_tip.point.x, 0.0);
nh_.param<double>("gripper_tip_y", gripper_tip.point.y, 0.0);
nh_.param<double>("gripper_tip_z", gripper_tip.point.z, 0.0);
gripper_tip.header.frame_id = tip_frame;
ROS_INFO("[calibrate] Initialized.");
}
template <typename PointT, typename Scalar> double
pcl::getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud,
Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
Eigen::Matrix<Scalar, 4, 1> centroid;
pcl::computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid);
EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> eigen_vects;
Eigen::Matrix<Scalar, 3, 1> eigen_vals;
pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals);
double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
transform.translation () = centroid.head (3);
transform.linear () = eigen_vects;
return (std::min (rel1, rel2));
}
int main(int argc, char** argv){
ros::init(argc, argv, "workspace_transformation");
ros::NodeHandle node;
ros::Rate loop_rate(loop_rate_in_hz);
// Topics
// Publishers
ros::Publisher pub = node.advertise<geometry_msgs::PoseStamped>("poseSlaveWorkspace", 1);
ros::Publisher pub_poseSlaveWSOrigin = node.advertise<geometry_msgs::PoseStamped>("poseSlaveWorkspaceOrigin", 1);
ros::Publisher pub_set_camera_pose = node.advertise<geometry_msgs::PoseStamped>("Set_ActiveCamera_Pose", 1);
pub_OmniForceFeedback = node.advertise<geometry_msgs::Vector3>("set_forces", 1);
// Subscribers
ros::Subscriber sub_PoseMasterWS = node.subscribe("poseMasterWorkspace", 1, &PoseMasterWSCallback);
ros::Subscriber sub_BaseMasterWS = node.subscribe("baseMasterWorkspace", 1, &BaseMasterWSCallback);
ros::Subscriber sub_BaseSlaveWS = node.subscribe("baseSlaveWorkspace", 1, &BaseSlaveWSCallback);
ros::Subscriber sub_OriginSlaveWS = node.subscribe("originSlaveWorkspace", 1, &OriginSlaveWSCallback);
ros::Subscriber sub_scale = node.subscribe("scale", 1, &scaleCallback);
ros::Subscriber subOmniButtons = node.subscribe("button", 1, &omniButtonCallback);
ros::Subscriber sub_HapticAngles = node.subscribe("angles", 1, &HapticAnglesCallback);
// Services
ros::ServiceServer service_server_algorithm = node.advertiseService("set_algorithm", algorithmSrvCallback);
// INITIALIZATION ------------------------------------------------------------------------
// Haptic
omni_white_button_pressed = omni_grey_button_pressed = first_haptic_angle_read = false;
// Algorithm
algorithm_type = 0;
for (unsigned int i=0; i<3; i++) scale[i] = 1.0;
m_init = s_init = mi_init = s0_init = algorithm_set = false;
dm << 0,0,0;
ds << 0,0,0;
pm_im1 << 0,0,0;
ps_im1 << 0,0,0;
vm_i << 0,0,0;
ps_0 << 0.0, 0.0, 0.0;
quats_0.x() = quats_0.y() = quats_0.z() = 0.0;
quats_0.w() = 1.0;
Rs_0 = quats_0.toRotationMatrix();
// Auxiliary pose
geometry_msgs::PoseStamped outputPose;
outputPose.pose.position.x = outputPose.pose.position.y = outputPose.pose.position.z = 0.0;
outputPose.pose.orientation.x = outputPose.pose.orientation.y = outputPose.pose.orientation.z = 0.0;
outputPose.pose.orientation.w = 1.0;
// Workspace boundaries
Xmin = -5.0;
Ymin = -5.0;
Zmin = 0.0;
Xmax = 5.0;
Ymax = 5.0;
Zmax = 2.0;
// Default camera pose
geometry_msgs::PoseStamped cameraPose;
cameraPose.pose.position.x = cameraPose.pose.position.y = cameraPose.pose.position.z = 0.0;
cameraPose.pose.orientation.x = cameraPose.pose.orientation.y = cameraPose.pose.orientation.z = 0.0;
cameraPose.pose.orientation.w = 1.0;
Eigen::Vector3d origin_cam = 10.0 * Eigen::Vector3d(1.0, 0.0, 0.5);
T_camPose_S.translation() = Eigen::Vector3d(0.0, 0.0, 1.0) + origin_cam;
Eigen::Vector3d eigen_cam_axis_z = origin_cam.normalized();
Eigen::Vector3d eigen_cam_axis_x = ( Eigen::Vector3d::UnitZ().cross( eigen_cam_axis_z ) ).normalized();
Eigen::Vector3d eigen_cam_axis_y = ( eigen_cam_axis_z.cross( eigen_cam_axis_x ) ).normalized();
T_camPose_S.linear() << eigen_cam_axis_x(0), eigen_cam_axis_y(0), eigen_cam_axis_z(0),
eigen_cam_axis_x(1), eigen_cam_axis_y(1), eigen_cam_axis_z(1),
eigen_cam_axis_x(2), eigen_cam_axis_y(2), eigen_cam_axis_z(2);
// Time management
period = 1.0/(double)loop_rate_in_hz;
timeval past_time_, current_time_;
gettimeofday(¤t_time_, NULL);
time_increment_ = 0;
// File management
std::ofstream WTdataRecord;
WTdataRecord.open("/home/users/josep.a.claret/data/WTdataRecord.txt", std::ofstream::trunc);
// UPDATE -------------------------------------------------------------------------------
while (ros::ok())
{
if (m_init && s_init && mi_init)
{
// Time management
// past_time_ = current_time_;
// gettimeofday(¤t_time_, NULL);
// time_increment_ = ( (current_time_.tv_sec*1e6 + current_time_.tv_usec) - (past_time_.tv_sec*1e6 + past_time_.tv_usec) ) / 1e6;
time_increment_ = period;
// Velocity computation
vm_i = (pm_i - pm_im1)/time_increment_;
ws_tf_alg_scaling_Bubble_smoothposrotRateControl_camRateControl_FormatJAC();
// std::cout << "-- VISUALIZATION DATA ---------------------------------------------------" << std::endl;
// std::cout << "alg: " << algorithm_type
// std::cout << " time inc: " << time_increment_*1000 << std::endl;
// std::cout << " pm_0: " << 1000*pm_0.transpose()<< std::endl;
// std::cout << " pm_im1: " << 1000*pm_im1.transpose()<< std::endl;
// std::cout << " pm_i: " << 1000*pm_i.transpose()<< std::endl;
// std::cout << " ps_0: " << 1000*ps_0.transpose()<< std::endl;
// std::cout << " ps_im1: " << 1000*ps_im1.transpose()<< std::endl;
// std::cout << " ps_i: " << 1000*ps_i.transpose()<< std::endl;
// std::cout << " dm: " << 1000*dm.transpose()<< std::endl;
// std::cout << " ds: " << 1000*ds.transpose()<< std::endl;
// std::cout << " vm_i: " << 1000*vm_i.transpose()<< std::endl;
// std::cout << "Rm_0" << std::endl;
// std::cout << Rm_0 << std::endl;
// std::cout << "Rm" << std::endl;
// std::cout << Rm << std::endl;
// std::cout << "Rm * ds" << std::endl;
// std::cout << Rm * ds << std::endl;
// std::cout << "Rs.transpose() * Rm * ds" << std::endl;
// std::cout << Rs.transpose() * Rm * ds << std::endl;
//
// std::cout << "Rm_i" << std::endl;
// std::cout << Rm_i << std::endl;
// std::cout << "Rs_i" << std::endl;
// std::cout << Rs_i << std::endl;
// std::cout << "quat Rs_i" << std::endl;
// std::cout << quats_i.x() << " " << quats_i.y() << " " << quats_i.z() << " " << quats_i.w() << std::endl;
pm_im1 = pm_i;
ps_im1 = ps_i;
// Send data ***********************************************
// Slave pose
outputPose.header.stamp = ros::Time::now();
outputPose.pose.position.x = ps_i(0,0);
outputPose.pose.position.y = ps_i(1,0);
outputPose.pose.position.z = ps_i(2,0);
outputPose.pose.orientation.x = quats_i.x();
outputPose.pose.orientation.y = quats_i.y();
outputPose.pose.orientation.z = quats_i.z();
outputPose.pose.orientation.w = quats_i.w();
pub.publish(outputPose);
// Slave origin pose
outputPose.header.stamp = ros::Time::now();
outputPose.pose.position.x = ps_0(0,0);
outputPose.pose.position.y = ps_0(1,0);
outputPose.pose.position.z = ps_0(2,0);
outputPose.pose.orientation.x = quats_0.x();
outputPose.pose.orientation.y = quats_0.y();
outputPose.pose.orientation.z = quats_0.z();
outputPose.pose.orientation.w = quats_0.w();
pub_poseSlaveWSOrigin.publish(outputPose);
}
// Camera pose
cameraPose.header.stamp = ros::Time::now();
cameraPose.pose.position.x = T_camPose_S.translation()(0);
cameraPose.pose.position.y = T_camPose_S.translation()(1);
cameraPose.pose.position.z = T_camPose_S.translation()(2);
cameraPose.pose.orientation.x = Quaternion<double>(T_camPose_S.linear()).x();
cameraPose.pose.orientation.y = Quaternion<double>(T_camPose_S.linear()).y();
cameraPose.pose.orientation.z = Quaternion<double>(T_camPose_S.linear()).z();
cameraPose.pose.orientation.w = Quaternion<double>(T_camPose_S.linear()).w();
pub_set_camera_pose.publish(cameraPose);
ros::spinOnce();
loop_rate.sleep();
}
WTdataRecord.close();
return 0;
}
void IterativeClosestPoint::execute() {
float error = std::numeric_limits<float>::max(), previousError;
uint iterations = 0;
// Get access to the two point sets
PointSetAccess::pointer accessFixedSet = ((PointSet::pointer)getStaticInputData<PointSet>(0))->getAccess(ACCESS_READ);
PointSetAccess::pointer accessMovingSet = ((PointSet::pointer)getStaticInputData<PointSet>(1))->getAccess(ACCESS_READ);
// Get transformations of point sets
AffineTransformation::pointer fixedPointTransform2 = SceneGraph::getAffineTransformationFromData(getStaticInputData<PointSet>(0));
Eigen::Affine3f fixedPointTransform;
fixedPointTransform.matrix() = fixedPointTransform2->matrix();
AffineTransformation::pointer initialMovingTransform2 = SceneGraph::getAffineTransformationFromData(getStaticInputData<PointSet>(1));
Eigen::Affine3f initialMovingTransform;
initialMovingTransform.matrix() = initialMovingTransform2->matrix();
// These matrices are Nx3
MatrixXf fixedPoints = accessFixedSet->getPointSetAsMatrix();
MatrixXf movingPoints = accessMovingSet->getPointSetAsMatrix();
Eigen::Affine3f currentTransformation = Eigen::Affine3f::Identity();
// Want to choose the smallest one as moving
bool invertTransform = false;
if(false && fixedPoints.cols() < movingPoints.cols()) {
reportInfo() << "switching fixed and moving" << Reporter::end;
// Switch fixed and moving
MatrixXf temp = fixedPoints;
fixedPoints = movingPoints;
movingPoints = temp;
invertTransform = true;
// Apply initial transformations
//currentTransformation = fixedPointTransform.getTransform();
movingPoints = fixedPointTransform*movingPoints.colwise().homogeneous();
fixedPoints = initialMovingTransform*fixedPoints.colwise().homogeneous();
} else {
// Apply initial transformations
//currentTransformation = initialMovingTransform.getTransform();
movingPoints = initialMovingTransform*movingPoints.colwise().homogeneous();
fixedPoints = fixedPointTransform*fixedPoints.colwise().homogeneous();
}
do {
previousError = error;
MatrixXf movedPoints = currentTransformation*(movingPoints.colwise().homogeneous());
// Match closest points using current transformation
MatrixXf rearrangedFixedPoints = rearrangeMatrixToClosestPoints(
fixedPoints, movedPoints);
// Get centroids
Vector3f centroidFixed = getCentroid(rearrangedFixedPoints);
//reportInfo() << "Centroid fixed: " << Reporter::end;
//reportInfo() << centroidFixed << Reporter::end;
Vector3f centroidMoving = getCentroid(movedPoints);
//reportInfo() << "Centroid moving: " << Reporter::end;
//reportInfo() << centroidMoving << Reporter::end;
Eigen::Transform<float, 3, Eigen::Affine> updateTransform = Eigen::Transform<float, 3, Eigen::Affine>::Identity();
if(mTransformationType == IterativeClosestPoint::RIGID) {
// Create correlation matrix H of the deviations from centroid
MatrixXf H = (movedPoints.colwise() - centroidMoving)*
(rearrangedFixedPoints.colwise() - centroidFixed).transpose();
// Do SVD on H
Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
// Estimate rotation as R=V*U.transpose()
MatrixXf temp = svd.matrixV()*svd.matrixU().transpose();
Matrix3f d = Matrix3f::Identity();
d(2,2) = sign(temp.determinant());
Matrix3f R = svd.matrixV()*d*svd.matrixU().transpose();
// Estimate translation
Vector3f T = centroidFixed - R*centroidMoving;
updateTransform.linear() = R;
updateTransform.translation() = T;
} else {
// Only translation
Vector3f T = centroidFixed - centroidMoving;
updateTransform.translation() = T;
}
// Update current transformation
currentTransformation = updateTransform*currentTransformation;
// Calculate RMS error
MatrixXf distance = rearrangedFixedPoints - currentTransformation*(movingPoints.colwise().homogeneous());
error = 0;
for(uint i = 0; i < distance.cols(); i++) {
error += pow(distance.col(i).norm(),2);
}
error = sqrt(error / distance.cols());
iterations++;
reportInfo() << "Error: " << error << Reporter::end;
// To continue, change in error has to be above min error change and nr of iterations less than max iterations
} while(previousError-error > mMinErrorChange && iterations < mMaxIterations);
if(invertTransform){
currentTransformation = currentTransformation.inverse();
}
mError = error;
mTransformation->matrix() = currentTransformation.matrix();
}
/**
* @function straightPath
* @brief Builds a straight path -- easy
*/
bool goWSOrient::straightPath( const Eigen::VectorXd &_startConfig,
const Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
std::vector<Eigen::VectorXd> &path )
{
//-- Copy input
startConfig = _startConfig;
goalPose = _goalPose;
goalPos = _goalPose.translation();
//-- Create needed variables
Eigen::VectorXd q; Eigen::VectorXd wsPos;
Eigen::Transform<double, 3, Eigen::Affine> T;
Eigen::MatrixXd Jc(6, 7); Eigen::MatrixXd Jcinv(7, 6);
Eigen::MatrixXd delta_q;
double ws_dist;
//-- Initialize variables
q = startConfig; path.push_back( q );
ws_dist = DBL_MAX;
//-- Loop
int trials = 0;
while( ws_dist > goalThresh )
{
//-- Get ws position
robinaLeftArm_fk( q, TWBase, Tee, T );
wsPos = T.translation();
Eigen::VectorXd wsOrient(3);
getRPY( T, wsOrient(0), wsOrient(1), wsOrient(2) );
//-- Get the Jacobian
robinaLeftArm_jc( q, TWBase, Tee, Jc );
std::cout<< " JC: " << std::endl << Jc << std::endl;
for( int i = 3; i < 6;i++ )
{ for( int j = 0; j < 7; j++ )
{
Jc(i,j) = Jc(i,j)*0.001;
}
}
std::cout<< " JC switch: " << std::endl << Jc << std::endl;
//-- Get the pseudo-inverse(easy way)
pseudoInv( 6, 7, Jc, Jcinv );
std::cout<< " JCW Inv: " << std::endl << Jcinv << std::endl;
Eigen::VectorXd goalOrient(3);
getRPY( goalPose, goalOrient(0), goalOrient(1), goalOrient(2) );
//-- Get delta (orientation + position)
Eigen::VectorXd delta(6);
delta.head(3) = (goalPos - wsPos); delta.tail(3) = (goalOrient - wsOrient);
//-- Get delta_q (jointspace)
delta_q = Jcinv*delta;
//-- Scale
double scal = stepSize/delta_q.norm();
delta_q = delta_q*scal;
//-- Add to q
q = q + delta_q;
//-- Push it
path.push_back( q );
//-- Check distance to goal
ws_dist = (goalPos - wsPos).norm();
printf(" -- Ws dist: %.3f \n", ws_dist );
trials++;
if( trials > maxTrials )
{ break; }
}
if( trials >= maxTrials )
{ path.clear();
printf(" --(!) Did not get to the goal . Thresh: %.3f Trials: %d \n", ws_dist, trials );
return false;}
else
{ printf(" -- Got to the goal . Thresh: %.3f Trials: %d \n", ws_dist, trials );
return true; }
}
typename PointMatcher<T>::TransformationParameters ErrorMinimizersImpl<T>::PointToPlaneWithCovErrorMinimizer::compute(
const DataPoints& filteredReading,
const DataPoints& filteredReference,
const OutlierWeights& outlierWeights,
const Matches& matches)
{
assert(matches.ids.rows() > 0);
// Fetch paired points
typename ErrorMinimizer::ErrorElements& mPts = this->getMatchedPoints(filteredReading, filteredReference, matches, outlierWeights);
const int dim = mPts.reading.features.rows();
const int nbPts = mPts.reading.features.cols();
// Adjust if the user forces 2D minimization on XY-plane
int forcedDim = dim - 1;
if(force2D && dim == 4)
{
mPts.reading.features.conservativeResize(3, Eigen::NoChange);
mPts.reading.features.row(2) = Matrix::Ones(1, nbPts);
mPts.reference.features.conservativeResize(3, Eigen::NoChange);
mPts.reference.features.row(2) = Matrix::Ones(1, nbPts);
forcedDim = dim - 2;
}
// Fetch normal vectors of the reference point cloud (with adjustment if needed)
const BOOST_AUTO(normalRef, mPts.reference.getDescriptorViewByName("normals").topRows(forcedDim));
// Note: Normal vector must be precalculated to use this error. Use appropriate input filter.
assert(normalRef.rows() > 0);
// Compute cross product of cross = cross(reading X normalRef)
const Matrix cross = this->crossProduct(mPts.reading.features, normalRef);
// wF = [weights*cross, weight*normals]
// F = [cross, normals]
Matrix wF(normalRef.rows()+ cross.rows(), normalRef.cols());
Matrix F(normalRef.rows()+ cross.rows(), normalRef.cols());
for(int i=0; i < cross.rows(); i++)
{
wF.row(i) = mPts.weights.array() * cross.row(i).array();
F.row(i) = cross.row(i);
}
for(int i=0; i < normalRef.rows(); i++)
{
wF.row(i + cross.rows()) = mPts.weights.array() * normalRef.row(i).array();
F.row(i + cross.rows()) = normalRef.row(i);
}
// Unadjust covariance A = wF * F'
const Matrix A = wF * F.transpose();
if (A.fullPivHouseholderQr().rank() != A.rows())
{
// TODO: handle that properly
//throw ConvergenceError("encountered singular while minimizing point to plane distance");
}
const Matrix deltas = mPts.reading.features - mPts.reference.features;
// dot product of dot = dot(deltas, normals)
Matrix dotProd = Matrix::Zero(1, normalRef.cols());
for(int i=0; i<normalRef.rows(); i++)
{
dotProd += (deltas.row(i).array() * normalRef.row(i).array()).matrix();
}
// b = -(wF' * dot)
const Vector b = -(wF * dotProd.transpose());
// Cholesky decomposition
Vector x(A.rows());
x = A.llt().solve(b);
// Transform parameters to matrix
Matrix mOut;
if(dim == 4 && !force2D)
{
Eigen::Transform<T, 3, Eigen::Affine> transform;
// IT IS NOT CORRECT TO USE EULER ANGLES!
// Rotation in Eular angles follow roll-pitch-yaw (1-2-3) rule
/*transform = Eigen::AngleAxis<T>(x(0), Eigen::Matrix<T,1,3>::UnitX())
* Eigen::AngleAxis<T>(x(1), Eigen::Matrix<T,1,3>::UnitY())
* Eigen::AngleAxis<T>(x(2), Eigen::Matrix<T,1,3>::UnitZ()); */
// Reverse roll-pitch-yaw conversion, very useful piece of knowledge, keep it with you all time!
/*const T pitch = -asin(transform(2,0));
const T roll = atan2(transform(2,1), transform(2,2));
const T yaw = atan2(transform(1,0) / cos(pitch), transform(0,0) / cos(pitch));
std::cerr << "d angles" << x(0) - roll << ", " << x(1) - pitch << "," << x(2) - yaw << std::endl;*/
transform = Eigen::AngleAxis<T>(x.head(3).norm(),x.head(3).normalized());
transform.translation() = x.segment(3, 3);
mOut = transform.matrix();
}
else
{
Eigen::Transform<T, 2, Eigen::Affine> transform;
transform = Eigen::Rotation2D<T> (x(0));
transform.translation() = x.segment(1, 2);
if(force2D)
{
mOut = Matrix::Identity(dim, dim);
mOut.topLeftCorner(2, 2) = transform.matrix().topLeftCorner(2, 2);
mOut.topRightCorner(2, 1) = transform.matrix().topRightCorner(2, 1);
}
else
{
mOut = transform.matrix();
}
}
this->covMatrix = this->estimateCovariance(filteredReading, filteredReference, matches, outlierWeights, mOut);
return mOut;
}
/**
* @function balancePath
* @brief Builds a straight path -- easy
*/
bool goWSOrient::balancePath( const Eigen::VectorXd &_startConfig,
const Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
std::vector<Eigen::VectorXd> &path )
{
srand( time(NULL) );
//-- Copy input
startConfig = _startConfig;
goalPose = _goalPose;
goalPos = _goalPose.translation();
//-- Create needed variables
Eigen::VectorXd q; Eigen::VectorXd wsPos;
Eigen::Transform<double, 3, Eigen::Affine> T;
Eigen::MatrixXd Jc(3, 7); Eigen::MatrixXd Jcinv(7, 3);
Eigen::MatrixXd delta_q;
double ws_dist;
//-- Initialize variables
q = startConfig; path.push_back( q );
ws_dist = DBL_MAX;
//-- Loop
int trials = 0;
//-- Get ws position
robinaLeftArm_fk( q, TWBase, Tee, T );
wsPos = T.translation();
Eigen::VectorXd wsOrient(3);
getRPY( T, wsOrient(0), wsOrient(1), wsOrient(2) );
Eigen::VectorXd phi(7);
Eigen::VectorXd dq(7);
Eigen::VectorXd qorig = q;
//-- Get the Jacobian
robinaLeftArm_j( q, TWBase, Tee, Jc );
//-- Get the pseudo-inverse(easy way)
pseudoInv( 3, 7, Jc, Jcinv );
int count = 0;
for( int i = 0; i < 1000; i++ )
{
for( int j = 0; j < 7; j++ )
{ phi[j] = rand()%6400 / 10000.0 ; }
//-- Get a null space projection displacement that does not affect the thing
dq = ( Eigen::MatrixXd::Identity(7,7) - Jcinv*Jc )*phi;
//-- Add to q
//q = qorig + dq;
Eigen::VectorXd newq;
newq = qorig + dq;
//-- Calculate distance
Eigen::Transform<double, 3, Eigen::Affine> Torig;
Eigen::Transform<double, 3, Eigen::Affine> Tphi;
robinaLeftArm_fk( newq, TWBase, Tee, Tphi );
robinaLeftArm_fk( qorig, TWBase, Tee, Torig );
double dist = ( Tphi.translation() - Torig.translation() ).norm();
if( dist < 0.015 )
{
count++;
q = newq;
printf("--> [%d] Distance to the original xyz is: %.3f \n", count, dist );
//-- Push it
path.push_back( q );
}
}
return true;
}