void MotionPlanningFrame::snapStateToFrame(robot_state::RobotState& state,
const Eigen::Affine3d& T_frame_world,
const std::string& link,
const Eigen::Affine3d& T_frame_link,
const std::string& group)
{
ROS_DEBUG_FUNCTION;
// std::cout << "T_frame_world:\n" << T_frame_world.matrix() << std::endl;
// std::cout << "T_frame_link:\n" << T_frame_link.matrix() << std::endl;
// Back out the desired link transform in global coordinates.
Eigen::Affine3d T_link_world = T_frame_world * T_frame_link.inverse();
// Snap to the frame using IK.
const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group);
// std::cout << "group: " << group << std::endl;
state.update();
trajectory_msgs::JointTrajectoryPoint point;
state.copyJointGroupPositions(jmg, point.positions);
ROS_DEBUG_STREAM("pre-ik positions:\n" << point);
geometry_msgs::Pose pose;
tf::poseEigenToMsg(T_link_world, pose);
state.setFromIK(jmg, pose, link);
state.update();
state.copyJointGroupPositions(jmg, point.positions);
ROS_DEBUG_STREAM("post-ik positions:\n" << point);
ROS_DEBUG_FUNCTION;
}
//this is a pretty general function:
// goal contains a desired tool pose;
// path is planned from current joint state to some joint state that achieves desired tool pose
bool ArmMotionInterface::rt_arm_plan_path_current_to_goal_pose() {
ROS_INFO("computing a cartesian trajectory to right-arm tool goal pose");
//unpack the goal pose:
goal_gripper_pose_right_ = cart_goal_.des_pose_gripper_right;
goal_gripper_affine_right_= transformPoseToEigenAffine3d(goal_gripper_pose_right_.pose);
ROS_INFO("tool frame goal: ");
display_affine(goal_gripper_affine_right_);
goal_flange_affine_right_ = goal_gripper_affine_right_ * A_tool_wrt_flange_.inverse();
ROS_INFO("flange goal");
display_affine(goal_flange_affine_right_);
Vectorq7x1 q_start;
q_start = get_jspace_start_right_arm_(); // choose last cmd, or current joint angles
path_is_valid_ = cartTrajPlanner_.cartesian_path_planner(q_start, goal_flange_affine_right_, optimal_path_);
if (path_is_valid_) {
baxter_traj_streamer_.stuff_trajectory(optimal_path_, des_trajectory_); //convert from vector of poses to trajectory message
computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec();
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::SUCCESS;
cart_result_.computed_arrival_time = computed_arrival_time_;
cart_move_as_.setSucceeded(cart_result_);
}
else {
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID;
cart_result_.computed_arrival_time = -1.0; //impossible arrival time
cart_move_as_.setSucceeded(cart_result_); //the communication was a success, but not the computation
}
return path_is_valid_;
}
void Irp6pInverseKinematic::updateHook() {
if (port_input_wrist_pose_.read(wrist_pose_) == RTT::NewData) {
Eigen::Affine3d trans;
tf::poseMsgToEigen(wrist_pose_, trans);
port_current_joint_position_.read(local_current_joints_);
inverse_kinematics_single_iteration(local_current_joints_, trans,
&local_desired_joints_);
port_output_joint_position_.write(local_desired_joints_);
} else if (port_input_end_effector_pose_.read(end_effector_pose_)
== RTT::NewData) {
port_tool_.read(tool_msgs_);
Eigen::Affine3d tool;
Eigen::Affine3d trans;
Eigen::Affine3d wrist_pose;
tf::poseMsgToEigen(end_effector_pose_, trans);
tf::poseMsgToEigen(tool_msgs_, tool);
wrist_pose = trans * tool.inverse();
port_current_joint_position_.read(local_current_joints_);
inverse_kinematics_single_iteration(local_current_joints_, wrist_pose,
&local_desired_joints_);
port_output_joint_position_.write(local_desired_joints_);
}
}
template <typename PointT, typename NormalT> bool
cpu_tsdf::TSDFVolumeOctree::integrateCloud (
const pcl::PointCloud<PointT> &cloud,
const pcl::PointCloud<NormalT> &normals,
const Eigen::Affine3d &trans)
{
Eigen::Affine3f trans_inv = trans.inverse ().cast<float> ();
// First, sample a few points and force their containing voxels to split
int px_step = 1;
int nsplit = 0;
for (size_t u = 0; u < cloud.width; u += px_step)
{
for (size_t v = 0; v < cloud.height; v += px_step)
{
const PointT &pt_surface_orig = cloud (u, v);
if (pcl_isnan (pt_surface_orig.z))
continue;
// Look at surroundings
int nstep = 0;
Eigen::Vector3f ray = pt_surface_orig.getVector3fMap ().normalized ();
for (int perm = 0; perm < num_random_splits_; perm++)
{
// Get containing voxels
PointT pt_trans;
float scale = (float)rand () / (float)RAND_MAX * 0.03;
Eigen::Vector3f noise = Eigen::Vector3f::Random ().normalized () * scale;;
if (perm == 0) noise *= 0;
pt_trans.getVector3fMap () = trans.cast<float> () * (pt_surface_orig.getVector3fMap ()+ noise);
OctreeNode* voxel = octree_->getContainingVoxel (pt_trans.x, pt_trans.y, pt_trans.z);
if (voxel != NULL)
{
while (voxel->getMinSize () > xsize_ / xres_)
{
nsplit++;
voxel->split ();
voxel = voxel->getContainingVoxel (pt_trans.x, pt_trans.y, pt_trans.z);
}
}
}
}
}
// Do Frustum Culling to get rid of unseen voxels
std::vector<cpu_tsdf::OctreeNode::Ptr> voxels_culled;
getFrustumCulledVoxels(trans, voxels_culled);
#pragma omp parallel for
for (size_t i = 0; i < voxels_culled.size (); i++)
{
updateVoxel (voxels_culled[i], cloud, normals, trans_inv);
}
// Cloud is no longer empty
is_empty_ = false;
return (true);
}
void ICPLocalization::addScanLineToPointCloud(Eigen::Affine3d body2Odo, Eigen::Affine3d body2World, Eigen::Affine3d laser2Body, const ::base::samples::LaserScan &scan_reading)
{
if(scansWithTransforms.size() == 0)
{
addLaserScan(body2Odo,body2World,laser2Body,scan_reading);
return;
}
/*
double max_rotation = 1.5 * conf_point_cloud.lines_per_point_cloud * conf_point_cloud.min_rotation_for_new_line;
double max_translation = 1.5 * conf_point_cloud.lines_per_point_cloud * conf_point_cloud.min_distance_travelled_for_new_line;
double max_head_movement = 1.5 * conf_point_cloud.lines_per_point_cloud * conf_point_cloud.min_rotation_head_for_new_line;
*/
bool add_laser_scan = true;
for( uint i = 0; i < scansWithTransforms.size(); i++)
{
Eigen::Affine3d diference( body2Odo.inverse() * scansWithTransforms.at(i).body2Odo );
Vector3d Ylaser2Body = laser2Body * Vector3d::UnitY() - laser2Body.translation();
Ylaser2Body.normalize();
Vector3d YlastLaser2Body = scansWithTransforms.back().laser2Body * Vector3d::UnitY() - scansWithTransforms.at(i).laser2Body.translation();
YlastLaser2Body.normalize();
double laserChange = acos(Ylaser2Body.dot(YlastLaser2Body));
double translation = diference.translation().norm();
double rotation = fabs(Eigen::AngleAxisd( diference.rotation() ).angle()) ;
add_laser_scan = add_laser_scan && ( rotation > conf_point_cloud.min_rotation_for_new_line || translation > conf_point_cloud.min_distance_travelled_for_new_line || laserChange > conf_point_cloud.min_rotation_head_for_new_line);
//if the distance is to big means the old laser scan is not consistent anymore.
// if( rotation > max_rotation|| translation > max_translation || laserChange > max_head_movement)
// {
// scansWithTransforms.erase(scansWithTransforms.begin() + i);
// scanCount--;
// std::cout << " IcpLocalization.cpp erasing old laser scan " << std::endl;
// }
/* std::cout <<" add new scan " << add_laser_scan << std::endl;
std::cout << "\t translation " << (translation > conf_point_cloud.min_distance_travelled_for_new_line)<< " " << translation << " > " << conf_point_cloud.min_distance_travelled_for_new_line << std::endl;
std::cout << "\t rotation " << (rotation > conf_point_cloud.min_rotation_for_new_line) << " " << rotation * 180 / M_PI << " > " << conf_point_cloud.min_rotation_for_new_line * 180 / M_PI << std::endl;
std::cout << "\t head " << (laserChange > conf_point_cloud.min_rotation_head_for_new_line) << " "<< laserChange * 180 / M_PI << " > " << conf_point_cloud.min_rotation_head_for_new_line * 180 / M_PI<< std::endl; */
if (!add_laser_scan)
break;
}
if ( add_laser_scan )
{
addLaserScan(body2Odo,body2World,laser2Body,scan_reading);
}
return;
}
void PolygonArrayTransformer::transformPolygon(const Eigen::Affine3d& transform,
const geometry_msgs::PolygonStamped& polygon,
geometry_msgs::PolygonStamped& result)
{
result.header = polygon.header;
result.header.frame_id = frame_id_;
for (size_t i = 0; i < polygon.polygon.points.size(); i++) {
Eigen::Vector4d point;
point[0] = polygon.polygon.points[i].x;
point[1] = polygon.polygon.points[i].y;
point[2] = polygon.polygon.points[i].z;
point[3] = 1; // homogenious
Eigen::Vector4d transformed_point_eigen = transform.inverse() * point;
geometry_msgs::Point32 transformed_point;
transformed_point.x = transformed_point_eigen[0];
transformed_point.y = transformed_point_eigen[1];
transformed_point.z = transformed_point_eigen[2];
result.polygon.points.push_back(transformed_point);
}
}
bool ArmMotionInterface::unpack_goal_pose(void) {
geometry_msgs::PoseStamped poseStamped_goal = request_.poseStamped_goal;
Eigen::Vector3d goal_origin;
goal_origin[0] = poseStamped_goal.pose.position.x;
goal_origin[1] = poseStamped_goal.pose.position.y;
goal_origin[2] = poseStamped_goal.pose.position.z;
a_tool_end_.translation() = goal_origin;
Eigen::Quaterniond quat;
quat.x() = poseStamped_goal.pose.orientation.x;
quat.y() = poseStamped_goal.pose.orientation.y;
quat.z() = poseStamped_goal.pose.orientation.z;
quat.w() = poseStamped_goal.pose.orientation.w;
Eigen::Matrix3d R_goal(quat);
a_tool_end_.linear() = R_goal;
a_flange_end_ = a_tool_end_ * A_tool_wrt_flange_.inverse();
return true;
}
bool CaptureStereoSynchronizer::checkNearPose(
const geometry_msgs::Pose& new_pose)
{
Eigen::Affine3d new_affine;
tf::poseMsgToEigen(new_pose, new_affine);
for (size_t i = 0; i < poses_.size(); i++) {
// convert pose into eigen
Eigen::Affine3d affine;
tf::poseMsgToEigen(poses_[i], affine);
// compute difference
Eigen::Affine3d diff = affine.inverse() * new_affine;
double positional_difference = diff.translation().norm();
if (positional_difference < positional_bin_size_) {
Eigen::AngleAxisd rotational_difference(diff.rotation());
if (rotational_difference.angle() < rotational_bin_size_) {
return true;
}
}
}
return false;
}
void inPointCallback(const geometry_msgs::Point& pt_msg) {
Eigen::Affine3d des_gripper1_wrt_base;
g_des_point(0) = pt_msg.x;
g_des_point(1) = pt_msg.y;
g_des_point(2) = pt_msg.z;
cout << "received des point = " << g_des_point.transpose() << endl;
g_des_gripper_affine1.translation() = g_des_point;
//convert this to base coords:
g_des_gripper1_wrt_base = g_affine_lcamera_to_psm_one.inverse() * g_des_gripper_affine1;
//try computing IK:
if (g_ik_solver.ik_solve(g_des_gripper1_wrt_base)) {
ROS_INFO("found IK soln");
g_got_new_pose = true;
g_q_vec1_start = g_q_vec1_goal;
g_q_vec1_goal = g_ik_solver.get_soln();
cout << g_q_vec1_goal.transpose() << endl;
g_trajectory_point1 = g_trajectory_point2; //former goal is new start
for (int i = 0; i < 6; i++) {
g_trajectory_point2.positions[i] = g_q_vec1_goal[i]; //leave psm2 angles alone; just update psm1 goal angles
}
//gripper_affines_wrt_camera.push_back(affine_gripper_frame_wrt_camera_frame_);
// des_trajectory.joint_names.push_back("right_s0"); //yada-yada should fill in names
g_des_trajectory.header.stamp = ros::Time::now();
g_des_trajectory.points[0] = g_des_trajectory.points[1]; //former goal is new start
g_des_trajectory.points[1] = g_trajectory_point2;
// copy traj to goal:
g_goal.trajectory = g_des_trajectory;
} else {
ROS_WARN("NO IK SOLN FOUND");
}
}
// static BITBOTS_INLINE Eigen::Matrix4d inverse_chain_matrix(const Eigen::Matrix4d chain_matrix) {
// Eigen::Matrix4d inverse;
// inverse.block<3, 3>(0, 0) = chain_matrix.block<3, 3>(0, 0).inverse();
// inverse.col(3).head<3>() = -(inverse.block<3, 3>(0, 0) * chain_matrix.col(3).head<3>());
// inverse.row(3) = Eigen::RowVector4d(0, 0, 0, 1);
// return inverse;
// }
static BITBOTS_INLINE Eigen::Affine3d inverse_chain_matrix(const Eigen::Affine3d& chain_matrix) {
return chain_matrix.inverse();
}
/**
* This is where the localization is done... not really callback in the offline version
*/
void callback(const sensor_msgs::LaserScan &scan, tf::Transform odo_pose, tf::Transform state_pose)
{
static int counter = 0;
counter++;
static tf::TransformListener tf_listener;
double looptime = TT.Tac();
TT.Tic();
fprintf(stderr,"Lt( %.1lfms %.1lfHz seq:%d) -",looptime*1000,1.0/looptime,scan.header.seq);
if(has_sensor_offset_set == false) return;
double gx,gy,gyaw,x,y,yaw;
///Get the ground truth
gyaw = tf::getYaw(state_pose.getRotation());
gx = state_pose.getOrigin().x();
gy = state_pose.getOrigin().y();
///Get the odometry
yaw = tf::getYaw(odo_pose.getRotation());
x = odo_pose.getOrigin().x();
y = odo_pose.getOrigin().y();
mrpt::utils::CTicTac tictac;
tictac.Tic();
int N =(scan.angle_max - scan.angle_min)/scan.angle_increment; ///< number of scan lines
/////
/// Pose conversions
////
Eigen::Affine3d T = getAsAffine(x,y,yaw);
Eigen::Affine3d Tgt = getAsAffine(gx,gy,gyaw);
/**
* We are now using the information from the ground truth to initialize the filter
*/
if(isFirstLoad){
fprintf(stderr,"Initializing to (%lf, %lf, %lf)\n",gx,gy,gyaw);
///Initialize the filter with 1m^2 variance in position and 20deg in heading
ndtmcl->initializeFilter(gx, gy,gyaw,1.0, 1.0, 20.0*M_PI/180.0, 450);
Told = T;
Todo = Tgt;
}
///Compute the differential motion between two time steps
Eigen::Affine3d Tmotion = Told.inverse() * T;
Todo = Todo*Tmotion;
Told = T;
///Get the laser pose with respect to the base
float dy =offy;
float dx = offx;
float alpha = atan2(dy,dx);
float L = sqrt(dx*dx+dy*dy);
///Laser pose in base frame
float lpx = L * cos(alpha);
float lpy = L * sin(alpha);
float lpa = offa;
///Laser scan to PointCloud transformed with respect to the base
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
for (int j=0;j<N;j++){
double r = scan.ranges[j];
if(r>=scan.range_min && r<scan.range_max && r>0.3 && r<20.0){
double a = scan.angle_min + j*scan.angle_increment;
pcl::PointXYZ pt;
pt.x = r*cos(a+lpa)+lpx;
pt.y = r*sin(a+lpa)+lpy;
pt.z = 0.1+0.02 * (double)rand()/(double)RAND_MAX;
cloud->push_back(pt);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Now we have the differential motion and pointcloud -- Lets do MCL
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ndtmcl->updateAndPredict(Tmotion, *cloud); ///< does the prediction, update and resampling steps (see ndt_mcl.hpp)
Eigen::Vector3d dm = ndtmcl->getMean(); ///<Maximum aposteriori estimate of the pose
Eigen::Matrix3d cov = ndtmcl->pf.getDistributionVariances(); ///distribution variances
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double Time = tictac.Tac();
fprintf(stderr,"Time elapsed %.1lfms (%lf %lf %lf) \n",Time*1000,dm[0],dm[1],dm[2]);
isFirstLoad = false;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//If visualization desired
#ifdef USE_VISUALIZATION_DEBUG
///The sensor pose in the global frame for visualization
Eigen::Vector3d origin(dm[0] + L * cos(dm[2]+alpha),dm[1] + L * sin(dm[2]+alpha),0.1);
Eigen::Affine3d ppos = getAsAffine(dm[0],dm[1],dm[2]);
//lslgeneric::transformPointCloudInPlace(Tgt, *cloud);
lslgeneric::transformPointCloudInPlace(ppos, *cloud);
mrpt::opengl::COpenGLScenePtr &scene = win3D.get3DSceneAndLock();
win3D.setCameraPointingToPoint(gx,gy,1);
if(counter%2000==0) gl_points->clear();
scene->clear();
scene->insert(plane);
addMap2Scene(ndtmcl->map, origin, scene);
addPoseCovariance(dm[0],dm[1],cov,scene);
addScanToScene(scene, origin, cloud);
addParticlesToWorld(ndtmcl->pf,Tgt.translation(),dm, Todo.translation());
scene->insert(gl_points);
scene->insert(gl_particles);
win3D.unlockAccess3DScene();
win3D.repaint();
if (win3D.keyHit())
{
mrpt::gui::mrptKeyModifier kmods;
int key = win3D.getPushedKey(&kmods);
}
#endif
}
FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess) {
unsigned int s_nr_data = src->data.cols();//std::min(int(src->data.cols()),int(500000));
unsigned int d_nr_data = dst->data.cols();
refinement->allow_regularization = true;
//printf("s_nr_data: %i d_nr_data: %i\n",s_nr_data,d_nr_data);
int stepy = std::max(1,int(d_nr_data)/100000);
Eigen::Matrix<double, 3, Eigen::Dynamic> Y;
Eigen::Matrix<double, 3, Eigen::Dynamic> N;
Y.resize(Eigen::NoChange,d_nr_data/stepy);
N.resize(Eigen::NoChange,d_nr_data/stepy);
unsigned int ycols = Y.cols();
for(unsigned int i = 0; i < d_nr_data/stepy; i++) {
Y(0,i) = dst->data(0,i*stepy);
Y(1,i) = dst->data(1,i*stepy);
Y(2,i) = dst->data(2,i*stepy);
N(0,i) = dst->normals(0,i*stepy);
N(1,i) = dst->normals(1,i*stepy);
N(2,i) = dst->normals(2,i*stepy);
}
/// Build kd-tree
//nanoflann::KDTreeAdaptor<Eigen::Matrix3Xd, 3, nanoflann::metric_L2_Simple> kdtree(Y);
Eigen::VectorXd DST_INORMATION = Eigen::VectorXd::Zero(Y.cols());
for(unsigned int i = 0; i < d_nr_data/stepy; i++) {
DST_INORMATION(i) = dst->information(0,i*stepy);
}
double s_mean_x = 0;
double s_mean_y = 0;
double s_mean_z = 0;
for(unsigned int i = 0; i < s_nr_data; i++) {
s_mean_x += src->data(0,i);
s_mean_y += src->data(1,i);
s_mean_z += src->data(2,i);
}
s_mean_x /= double(s_nr_data);
s_mean_y /= double(s_nr_data);
s_mean_z /= double(s_nr_data);
double d_mean_x = 0;
double d_mean_y = 0;
double d_mean_z = 0;
for(unsigned int i = 0; i < d_nr_data; i++) {
d_mean_x += dst->data(0,i);
d_mean_y += dst->data(1,i);
d_mean_z += dst->data(2,i);
}
d_mean_x /= double(d_nr_data);
d_mean_y /= double(d_nr_data);
d_mean_z /= double(d_nr_data);
double stop = 0.00001;
Eigen::Affine3d Ymean = Eigen::Affine3d::Identity();
Ymean(0,3) = d_mean_x;
Ymean(1,3) = d_mean_y;
Ymean(2,3) = d_mean_z;
Eigen::Affine3d Xmean = Eigen::Affine3d::Identity();
Xmean(0,3) = s_mean_x;
Xmean(1,3) = s_mean_y;
Xmean(2,3) = s_mean_z;
std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > all_X;
std::vector< Eigen::Affine3d > all_res;
std::vector< int > count_X;
std::vector< float > score_X;
std::vector< std::vector< Eigen::VectorXd > > all_starts;
int stepxsmall = std::max(1,int(s_nr_data)/250);
Eigen::VectorXd Wsmall (s_nr_data/stepxsmall);
for(unsigned int i = 0; i < s_nr_data/stepxsmall; i++) {
Wsmall(i) = src->information(0,i*stepxsmall);
}
double sumtime = 0;
double sumtimeSum = 0;
double sumtimeOK = 0;
int r = 0;
refinement->viewer = viewer;
refinement->visualizationLvl = 0;
//for(int r = 0; r < 1000; r++){
// while(true){
// double rx = 2.0*M_PI*0.0001*double(rand()%10000);
// double ry = 2.0*M_PI*0.0001*double(rand()%10000);
// double rz = 2.0*M_PI*0.0001*double(rand()%10000);
//double stop = 0;
double step = 0.1+2.0*M_PI/5;
for(double rx = 0; rx < 2.0*M_PI; rx += step) {
for(double ry = 0; ry < 2.0*M_PI; ry += step)
for(double rz = 0; rz < 2.0*M_PI; rz += step) {
printf("rx: %f ry: %f rz: %f\n",rx,ry,rz);
double start = getTime();
double meantime = 999999999999;
if(r != 0) {
meantime = sumtimeSum/double(sumtimeOK+1.0);
}
refinement->maxtime = std::min(0.5,3*meantime);
Eigen::VectorXd startparam = Eigen::VectorXd(3);
startparam(0) = rx;
startparam(1) = ry;
startparam(2) = rz;
Eigen::Affine3d randomrot = Eigen::Affine3d::Identity();
randomrot = Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ());
Eigen::Affine3d current_guess = Ymean*randomrot*Xmean.inverse();//*Ymean;
refinement->target_points = 250;
FusionResults fr = refinement->getTransform(current_guess.matrix());
//fr.timeout = timestopped;
double stoptime = getTime();
sumtime += stoptime-start;
if(!fr.timeout) {
sumtimeSum += stoptime-start;
sumtimeOK++;
}
//printf("sumtime: %f\n",sumtime);
stop = fr.score;
Eigen::Matrix4d m = fr.guess;
current_guess = m;//fr.guess;
float m00 = current_guess(0,0);
float m01 = current_guess(0,1);
float m02 = current_guess(0,2);
float m03 = current_guess(0,3);
float m10 = current_guess(1,0);
float m11 = current_guess(1,1);
float m12 = current_guess(1,2);
float m13 = current_guess(1,3);
float m20 = current_guess(2,0);
float m21 = current_guess(2,1);
float m22 = current_guess(2,2);
float m23 = current_guess(2,3);
Eigen::Matrix<double, 3, Eigen::Dynamic> Xsmall;
Xsmall.resize(Eigen::NoChange,s_nr_data/stepxsmall);
for(unsigned int i = 0; i < s_nr_data/stepxsmall; i++) {
float x = src->data(0,i*stepxsmall);
float y = src->data(1,i*stepxsmall);
float z = src->data(2,i*stepxsmall);
Xsmall(0,i) = m00*x + m01*y + m02*z + m03;
Xsmall(1,i) = m10*x + m11*y + m12*z + m13;
Xsmall(2,i) = m20*x + m21*y + m22*z + m23;
}
bool exists = false;
for(unsigned int ax = 0; ax < all_X.size(); ax++) {
Eigen::Matrix<double, 3, Eigen::Dynamic> axX = all_X[ax];
Eigen::Affine3d axT = all_res[ax];
double diff = (Xsmall-axX).colwise().norm().mean();
if(diff < 20*stop) {
count_X[ax]++;
all_starts[ax].push_back(startparam);
int count = count_X[ax];
float score = score_X[ax];
std::vector< Eigen::VectorXd > starts = all_starts[ax];
for(int bx = ax-1; bx >= 0; bx--) {
if(count_X[bx] < count_X[bx+1]) {
count_X[bx+1] = count_X[bx];
score_X[bx+1] = score_X[bx];
all_X[bx+1] = all_X[bx];
all_starts[bx+1] = all_starts[bx];
all_res[bx+1] = all_res[bx];
all_X[bx] = axX;
count_X[bx] = count;
score_X[bx] = score;
all_starts[bx] = starts;
all_res[bx] = axT;
} else {
break;
}
}
exists = true;
break;
}
}
if(!exists) {
all_X.push_back(Xsmall);
count_X.push_back(1);
score_X.push_back(stop);
all_starts.push_back(std::vector< Eigen::VectorXd >());
all_starts.back().push_back(startparam);
all_res.push_back(current_guess);
}
r++;
}
}
FusionResults fr = FusionResults();
refinement->allow_regularization = false;
int tpbef = refinement->target_points;
refinement->target_points = 2000;
for(unsigned int ax = 0; ax < all_X.size(); ax++) {
Eigen::Matrix4d np = all_res[ax].matrix();
refinement->visualizationLvl = visualizationLvl;
if(ax < 25) {
double start = getTime();
FusionResults fr1 = refinement->getTransform(np);
double stop = getTime();
np = fr1.guess;
}
refinement->visualizationLvl = 0;
fr.candidates.push_back(np);
fr.counts.push_back(count_X[ax]);
fr.scores.push_back(1.0/score_X[ax]);
}
refinement->target_points = tpbef;
return fr;
}
double transformationdiff(Eigen::Affine3d & A, Eigen::Affine3d & B, double rotationweight) {
Eigen::Affine3d C = A.inverse()*B;
double r = fabs(1-C(0,0))+fabs(C(0,1))+fabs(C(0,2)) + fabs(C(1,0))+fabs(1-C(1,1))+fabs(C(1,2)) + fabs(C(2,0))+fabs(C(2,1))+fabs(1-C(2,2));
double t = sqrt(C(0,3)*C(0,3)+C(1,3)*C(1,3)+C(2,3)*C(2,3));
return r*rotationweight+t;
}
void callback(const PointPlusConstPtr& robot_point, const PointPlusConstPtr& mass_point)
{
ROS_DEBUG("Synchronized Callback triggered");
puppeteer_msgs::PointPlus r_pt, m_pt;
// if not in run or calibrate, just exit
if (run_system_logic())
return;
// let's first correct for the radii of the objects:
r_pt = *robot_point;
r_pt = correct_vals(r_pt, DEFAULT_ROBOT_RADIUS);
m_pt = *mass_point;
m_pt = correct_vals(m_pt, DEFAULT_MASS_RADIUS);
if (calibrated_flag == false)
{
if (calibrate_count == 0)
{
calibrate_count++;
robot_cal_pos << 0, 0, 0;
mass_cal_pos << 0, 0, 0;
robot_start_pos << 0, 0, 0;
mass_start_pos << 0, 0, 0;
return;
}
else if (calibrate_count <= NUM_CALIBRATES)
{
ROS_INFO_THROTTLE(1, "Calibrating...");
if (!m_pt.error && !r_pt.error)
{
// then we got both objects successfully:
robot_cal_pos(0) += r_pt.x;
robot_cal_pos(1) += r_pt.y;
robot_cal_pos(2) += r_pt.z;
mass_cal_pos(0) += m_pt.x;
mass_cal_pos(1) += m_pt.y;
mass_cal_pos(2) += m_pt.z;
calibrate_count++;
}
return;
}
else
{
ROS_INFO("Calibration completed successfully!");
// if here, calibration is done!
// first find averages:
robot_cal_pos = (robot_cal_pos/((double) NUM_CALIBRATES));
mass_cal_pos = (mass_cal_pos/((double) NUM_CALIBRATES));
// now set start positions:
mass_start_pos << q0.xm, q0.ym, 0;
robot_start_pos << q0.xr, H0, 0;
// now get the transform:
mass_cal_pos -= mass_start_pos;
robot_cal_pos -= robot_start_pos;
// let's check if there is an error here:
////////////////////////
// ERROR CHECK //
////////////////////////
// if no error:
// cal_pos = (mass_cal_pos + robot_cal_pos)/2.0;
cal_pos = mass_cal_pos;
// reset vars:
calibrate_count = 0;
calibrated_flag = true;
}
}
// send global frame transforms:
send_global_transforms(m_pt.header.stamp);
// transform both of the points into the
// optimization_frame, build the output message, and
// publish
tf::Transform transform;
PlanarSystemConfig qmeas;
Eigen::Affine3d gwo;
Eigen::Vector3d robot_trans, mass_trans;
transform.setOrigin(tf::Vector3(cal_pos(0),
cal_pos(1), cal_pos(2)));
transform.setRotation(tf::Quaternion(0,0,0,1));
tf::TransformTFToEigen(transform, gwo);
gwo = gwo.inverse();
// transform robot:
robot_trans << r_pt.x, r_pt.y, r_pt.z;
robot_trans = gwo*robot_trans;
// transform mass:
mass_trans << m_pt.x, m_pt.y, m_pt.z;
mass_trans = gwo*mass_trans;
// calculate string length:
mass_trans(2) = 0; robot_trans(2) = 0;
double rad = (mass_trans - robot_trans).norm();
qmeas.xm = mass_trans(0);
qmeas.ym = mass_trans(1);
qmeas.xr = robot_trans(0);
qmeas.r = rad;
qmeas.mass_err = m_pt.error;
qmeas.robot_err = r_pt.error;
qmeas.header.stamp = m_pt.header.stamp;
qmeas.header.frame_id = "optimization_frame";
config_pub.publish(qmeas);
return;
}
void CartMoveActionServer::executeCB(const actionlib::SimpleActionServer<cwru_action::cart_moveAction>::GoalConstPtr& goal) {
ROS_INFO("in executeCB of CartMoveActionServer");
cart_result_.err_code=0;
cart_move_as_.isActive();
//unpack the necessary info:
gripper_ang1_ = goal->gripper_jaw_angle1;
gripper_ang2_ = goal->gripper_jaw_angle2;
arrival_time_ = goal->move_time;
// interpret the desired gripper poses:
geometry_msgs::PoseStamped des_pose_gripper1 = goal->des_pose_gripper1;
geometry_msgs::PoseStamped des_pose_gripper2 = goal->des_pose_gripper2;
// convert the above to affine objects:
des_gripper1_affine_wrt_lcamera_ = transformPoseToEigenAffine3d(des_pose_gripper1.pose);
cout<<"gripper1 desired pose; "<<endl;
cout<<des_gripper1_affine_wrt_lcamera_.linear()<<endl;
cout<<"origin: "<<des_gripper1_affine_wrt_lcamera_.translation().transpose()<<endl;
des_gripper2_affine_wrt_lcamera_ = transformPoseToEigenAffine3d(des_pose_gripper2.pose);
cout<<"gripper2 desired pose; "<<endl;
cout<<des_gripper2_affine_wrt_lcamera_.linear()<<endl;
cout<<"origin: "<<des_gripper2_affine_wrt_lcamera_.translation().transpose()<<endl;
//do IK to convert these to joint angles:
//Eigen::VectorXd q_vec1,q_vec2;
Vectorq7x1 q_vec1,q_vec2;
q_vec1.resize(7);
q_vec2.resize(7);
trajectory_msgs::JointTrajectory des_trajectory; // an empty trajectory
des_trajectory.points.clear(); // can clear components, but not entire trajectory_msgs
des_trajectory.joint_names.clear(); //could put joint names in...but I assume a fixed order and fixed size, so this is unnecessary
// if using wsn's trajectory streamer action server
des_trajectory.header.stamp = ros::Time::now();
trajectory_msgs::JointTrajectoryPoint trajectory_point; //,trajectory_point2;
trajectory_point.positions.resize(14);
ROS_INFO("\n");
ROS_INFO("stored previous command to gripper one: ");
cout<<gripper1_affine_last_commanded_pose_.linear()<<endl;
cout<<"origin: "<<gripper1_affine_last_commanded_pose_.translation().transpose()<<endl;
// first, reiterate previous command:
// this could be easier, if saved previous joint-space trajectory point...
des_gripper_affine1_ = affine_lcamera_to_psm_one_.inverse()*gripper1_affine_last_commanded_pose_; //previous pose
ik_solver_.ik_solve(des_gripper_affine1_); //convert desired pose into equiv joint displacements
q_vec1 = ik_solver_.get_soln();
q_vec1(6) = last_gripper_ang1_; // include desired gripper opening angle
des_gripper_affine2_ = affine_lcamera_to_psm_two_.inverse()*gripper2_affine_last_commanded_pose_; //previous pose
ik_solver_.ik_solve(des_gripper_affine2_); //convert desired pose into equiv joint displacements
q_vec2 = ik_solver_.get_soln();
cout<<"q_vec1 of stored pose: "<<endl;
for (int i=0;i<6;i++) {
cout<<q_vec1[i]<<", ";
}
cout<<endl;
q_vec2(6) = last_gripper_ang2_; // include desired gripper opening angle
for (int i=0;i<7;i++) {
trajectory_point.positions[i] = q_vec1(i);
trajectory_point.positions[i+7] = q_vec2(i);
}
cout<<"start traj pt: "<<endl;
for (int i=0;i<14;i++) {
cout<<trajectory_point.positions[i]<<", ";
}
cout<<endl;
trajectory_point.time_from_start = ros::Duration(0.0); // start time set to 0
// PUSH IN THE START POINT:
des_trajectory.points.push_back(trajectory_point);
// compute and append the goal point, in joint space trajectory:
des_gripper_affine1_ = affine_lcamera_to_psm_one_.inverse()*des_gripper1_affine_wrt_lcamera_;
ROS_INFO("desired gripper one location in base frame: ");
cout<<"gripper1 desired pose; "<<endl;
cout<<des_gripper_affine1_.linear()<<endl;
cout<<"origin: "<<des_gripper_affine1_.translation().transpose()<<endl;
ik_solver_.ik_solve(des_gripper_affine1_); //convert desired pose into equiv joint displacements
q_vec1 = ik_solver_.get_soln();
q_vec1(6) = gripper_ang1_; // include desired gripper opening angle
des_gripper_affine2_ = affine_lcamera_to_psm_two_.inverse()*des_gripper2_affine_wrt_lcamera_;
ik_solver_.ik_solve(des_gripper_affine2_); //convert desired pose into equiv joint displacements
q_vec2 = ik_solver_.get_soln();
cout<<"q_vec1 of goal pose: "<<endl;
for (int i=0;i<6;i++) {
cout<<q_vec1[i]<<", ";
}
cout<<endl;
q_vec2(6) = gripper_ang2_;
for (int i=0;i<7;i++) {
trajectory_point.positions[i] = q_vec1(i);
trajectory_point.positions[i+7] = q_vec2(i);
}
trajectory_point.time_from_start = ros::Duration(arrival_time_);
cout<<"goal traj pt: "<<endl;
for (int i=0;i<14;i++) {
cout<<trajectory_point.positions[i]<<", ";
}
cout<<endl;
des_trajectory.points.push_back(trajectory_point);
js_goal_.trajectory = des_trajectory;
// Need boost::bind to pass in the 'this' pointer
// see example: http://library.isr.ist.utl.pt/docs/roswiki/actionlib_tutorials%282f%29Tutorials%282f%29Writing%2820%29a%2820%29Callback%2820%29Based%2820%29Simple%2820%29Action%2820%29Client.html
// ac.sendGoal(goal,
// boost::bind(&MyNode::doneCb, this, _1, _2),
// Client::SimpleActiveCallback(),
// Client::SimpleFeedbackCallback());
js_action_client_.sendGoal(js_goal_, boost::bind(&CartMoveActionServer::js_doneCb_,this,_1,_2)); // we could also name additional callback functions here, if desired
// action_client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb); //alt--more callback funcs possible
double t_timeout=arrival_time_+2.0; //wait 2 sec longer than expected duration of move
bool finished_before_timeout = js_action_client_.waitForResult(ros::Duration(t_timeout));
//bool finished_before_timeout = action_client.waitForResult(); // wait forever...
if (!finished_before_timeout) {
ROS_WARN("joint-space interpolation result is overdue ");
} else {
ROS_INFO("finished before timeout");
}
ROS_INFO("completed callback" );
cart_move_as_.setSucceeded(cart_result_); // tell the client that we were successful acting on the request, and return the "result" message
//let's remember the last pose commanded, so we can use it as start pose for next move
gripper1_affine_last_commanded_pose_ = des_gripper1_affine_wrt_lcamera_;
gripper2_affine_last_commanded_pose_ = des_gripper2_affine_wrt_lcamera_;
//and the jaw opening angles:
last_gripper_ang1_=gripper_ang1_;
last_gripper_ang2_=gripper_ang2_;
}
/**
* Test if the delta of the provided transformations is greater in
* either distance or angle than the threshold.
*
* @param a2b the initial transformation from A to B
* @param aprime2b the next transformation from A' to B
*
* @result true if the transformation A' to A is greater than the stored thresholds
*/
bool test( const Eigen::Affine3d& a2b, const Eigen::Affine3d& aprime2b )
{
return test( a2b.inverse() * aprime2b );
}
int do_inits() {
Eigen::Matrix3d R;
Eigen::Vector3d nvec, tvec, bvec, tip_pos;
bvec << 0, 0, 1; //default for testing: gripper points "down"
nvec << 1, 0, 0;
tvec = bvec.cross(nvec);
R.col(0) = nvec;
R.col(1) = tvec;
R.col(2) = bvec;
g_des_gripper_affine1.linear() = R;
tip_pos << -0.15, -0.03, 0.07;
g_des_gripper_affine1.translation() = tip_pos; //will change this, but start w/ something legal
//hard-coded camera-to-base transform, useful for simple testing/debugging
g_affine_lcamera_to_psm_one.translation() << -0.155, -0.03265, 0.0;
nvec << -1, 0, 0;
tvec << 0, 1, 0;
bvec << 0, 0, -1;
//Eigen::Matrix3d R;
R.col(0) = nvec;
R.col(1) = tvec;
R.col(2) = bvec;
g_affine_lcamera_to_psm_one.linear() = R;
g_q_vec1_start.resize(7);
g_q_vec1_goal.resize(7);
g_q_vec2_start.resize(7);
g_q_vec2_goal.resize(7);
g_des_gripper1_wrt_base = g_affine_lcamera_to_psm_one.inverse() * g_des_gripper_affine1;
g_ik_solver.ik_solve(g_des_gripper1_wrt_base); // compute IK:
g_q_vec1_goal = g_ik_solver.get_soln();
g_q_vec1_start = g_q_vec1_goal;
g_q_vec2_start << 0, 0, 0, 0, 0, 0, 0; //just put gripper 2 at home position
g_q_vec2_goal << 0, 0, 0, 0, 0, 0, 0;
//repackage q's into a trajectory;
//populate a goal message for action server;
// initialize with 2 poses that are identical
g_trajectory_point1.positions.clear();
g_trajectory_point2.positions.clear();
//resize these:
for (int i=0;i<14;i++) {
g_trajectory_point1.positions.push_back(0.0);
g_trajectory_point2.positions.push_back(0.0);
}
for (int i = 0; i < 7; i++) {
g_trajectory_point1.positions[i] = g_q_vec1_start[i];
g_trajectory_point1.positions[i + 7] = g_q_vec2_start[i];
//should fix up jaw-opening values...do this later
}
g_trajectory_point1.time_from_start = ros::Duration(0.0);
g_trajectory_point2.time_from_start = ros::Duration(2.0);
g_des_trajectory.points.clear(); // can clear components, but not entire trajectory_msgs
g_des_trajectory.joint_names.clear();
// des_trajectory.joint_names.push_back("right_s0"); //yada-yada should fill in names
g_des_trajectory.header.stamp = ros::Time::now();
g_des_trajectory.points.push_back(g_trajectory_point1); // first point of the trajectory
g_des_trajectory.points.push_back(g_trajectory_point2);
// copy traj to goal:
g_goal.trajectory = g_des_trajectory;
g_got_new_pose = true; //send robot to start pose
ROS_INFO("getting transforms from camera to PSMs");
tf::TransformListener tfListener;
tf::StampedTransform tfResult_one, tfResult_two;
bool tferr = true;
int ntries = 0;
ROS_INFO("waiting for tf between base and right_hand...");
while (tferr) {
if (ntries > 5) break; //give up and accept default after this many tries
tferr = false;
try {
//The direction of the transform returned will be from the target_frame to the source_frame.
//Which if applied to data, will transform data in the source_frame into the target_frame. See tf/CoordinateFrameConventions#Transform_Direction
tfListener.lookupTransform("left_camera_optical_frame", "one_psm_base_link", ros::Time(0), tfResult_one);
//tfListener.lookupTransform("left_camera_optical_frame", "two_psm_base_link", ros::Time(0), tfResult_two);
} catch (tf::TransformException &exception) {
ROS_WARN("%s", exception.what());
tferr = true;
ros::Duration(0.5).sleep(); // sleep for half a second
ros::spinOnce();
ntries++;
}
}
//default transform: need to match this up to camera calibration!
if (tferr) {
g_affine_lcamera_to_psm_one.translation() << -0.155, -0.03265, 0.0;
Eigen::Vector3d nvec, tvec, bvec;
nvec << -1, 0, 0;
tvec << 0, 1, 0;
bvec << 0, 0, -1;
Eigen::Matrix3d R;
R.col(0) = nvec;
R.col(1) = tvec;
R.col(2) = bvec;
g_affine_lcamera_to_psm_one.linear() = R;
g_affine_lcamera_to_psm_one.linear() = R;
g_affine_lcamera_to_psm_one.translation() << 0.145, -0.03265, 0.0;
ROS_WARN("using default transform");
} else {
ROS_INFO("tf is good");
//affine_lcamera_to_psm_one is the position/orientation of psm1 base frame w/rt left camera link frame
// need to extend this to camera optical frame
g_affine_lcamera_to_psm_one = transformTFToEigen(tfResult_one);
//affine_lcamera_to_psm_two = transformTFToEigen(tfResult_two);
}
ROS_INFO("transform from left camera to psm one:");
cout << g_affine_lcamera_to_psm_one.linear() << endl;
cout << g_affine_lcamera_to_psm_one.translation().transpose() << endl;
//ROS_INFO("transform from left camera to psm two:");
//cout << affine_lcamera_to_psm_two.linear() << endl;
//cout << affine_lcamera_to_psm_two.translation().transpose() << endl;
ROS_INFO("done w/ inits");
return 0;
}
// This function is responsible for converting the data from the
// kinect into an odometry message, and tranforming it to the same
// frame that the ekf uses
void send_kinect_estimate(geometry_msgs::PointStamped pt,
geometry_msgs::PointStamped ptlast,
int index, int op)
{
ROS_DEBUG("send_kinect_estimate triggered");
Eigen::Affine3d gwo;
Eigen::Vector3d tmp_point;
tf::Transform transform;
geometry_msgs::PointStamped transpt, transptlast;
ros::Time tstamp = pt.header.stamp;
// set transform parameters
transform.setOrigin(tf::Vector3(cal_pos(0),
cal_pos(1), cal_pos(2)));
transform.setRotation(tf::Quaternion(0,0,0,1));
// convert to Eigen:
tf::TransformTFToEigen(transform, gwo);
gwo = gwo.inverse();
tmp_point << pt.point.x, pt.point.y, pt.point.z;
tmp_point = gwo*tmp_point;
transpt.header.frame_id = "optimization_frame";
transpt.header.stamp = tstamp;
transpt.point.x = tmp_point(0);
transpt.point.y = tmp_point(1);
transpt.point.z = tmp_point(2);
tmp_point << ptlast.point.x, ptlast.point.y, ptlast.point.z;
tmp_point = gwo*tmp_point;
transptlast.header.frame_id = "optimization_frame";
transptlast.header.stamp = tstamp;
transptlast.point.x = tmp_point(0);
transptlast.point.y = tmp_point(1);
transptlast.point.z = tmp_point(2);
ROS_DEBUG("Done transforming PointStamped messages");
// Let's first get the transform from /optimization_frame
// to /map
tf::StampedTransform trans_stamped;
geometry_msgs::PointStamped tmp;
try{
tf.lookupTransform(
"map", "optimization_frame",
tstamp, trans_stamped);
tf.transformPoint("map", transpt, tmp);
}
catch(tf::TransformException& ex){
ROS_ERROR(
"Error trying to lookupTransform from /map "
"to /optimization_frame: %s", ex.what());
return;
}
// Now we can publish the Kinect's estimate of the robot's
// pose
std::stringstream ss;
ss << "base_footprint_kinect_robot_" << index;
kin_pose[index].header.stamp = ros::Time::now();
kin_pose[index].header.frame_id = "map";
kin_pose[index].child_frame_id = ss.str();
tmp.point.z = 0.0;
kin_pose[index].pose.pose.position = tmp.point;
double theta = 0.0;
if (op == 2)
{
theta = atan2(transpt.point.x-
transptlast.point.x,
transpt.point.z-
transptlast.point.z);
theta = clamp_angle(theta-M_PI/2.0);
ROS_DEBUG("Calculated angle = %f",theta);
}
else
{
theta = robot_start_ori[index];
theta = clamp_angle(-theta);
ROS_DEBUG("predetermined angle = %f",theta);
}
geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(theta);
kin_pose[index].pose.pose.orientation = quat;
// Let's check if this is a "bad" point
if (bad_array[index])
{
boost::array<double,36ul> tmpcov;
for (int i=0; i<36; i++)
tmpcov[i] = kincov[i]*1000.0;
kin_pose[index].pose.covariance = tmpcov;
}
else
kin_pose[index].pose.covariance = kincov;
ROS_DEBUG("Done filling in Odometry message");
// Now let's publish the estimated pose as a
// nav_msgs/Odometry message on a topic called /vo
ROS_DEBUG("publishing /vo for robot %d", index+1);
robots_pub[index].publish(kin_pose[index]);
// now, let's publish the transforms that goes along with it
geometry_msgs::TransformStamped kin_trans;
tf::Quaternion q1, q2;
q1 = tf::createQuaternionFromYaw(theta);
q2 = tf::Quaternion(1.0,0,0,0);
q1 = q1*q2;
tf::quaternionTFToMsg(q1, quat);
kin_trans.header.stamp = tstamp;
kin_trans.header.frame_id = kin_pose[index].header.frame_id;
kin_trans.child_frame_id = kin_pose[index].child_frame_id;
kin_trans.transform.translation.x = kin_pose[index].pose.pose.position.x;
kin_trans.transform.translation.y = kin_pose[index].pose.pose.position.y;
kin_trans.transform.translation.z = kin_pose[index].pose.pose.position.z;
kin_trans.transform.rotation = quat;
ROS_DEBUG("Sending transform for output of estimator node");
br.sendTransform(kin_trans);
return;
}
pcl::PointCloud<pcl::PointNormal>::Ptr TSDFVolumeOctree::renderView (const Eigen::Affine3d& trans, int downsampleBy) const {
int new_width = image_width_ / downsampleBy;
int new_height = image_height_ / downsampleBy;
double new_fx = focal_length_x_ / downsampleBy;
double new_fy = focal_length_y_ / downsampleBy;
double new_cx = principal_point_x_ / downsampleBy;
double new_cy = principal_point_y_ / downsampleBy;
pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal> (new_width, new_height));
cloud->is_dense = false;
float min_step = max_dist_neg_ * 3/4.;
#pragma omp parallel for
for (size_t i = 0; i < cloud->size (); ++i) {
size_t x = i % new_width;
size_t y = i / new_width;
//Raytrace
pcl::PointNormal& pt = cloud->operator() (x,y);
bool found_crossing = false;
Eigen::Vector3f du ( (x - new_cx)/new_fx,
(y - new_cy)/new_fy,
1);
du.normalize ();
//Apply transform -- rotate the ray, start at the offset
du = trans.rotation ().cast<float> () * du;
Eigen::Vector3f p = trans.translation ().cast<float> ();
//Preallocate
int x_i, y_i, z_i;
float d;
float w;
float last_w = 0;
float last_d = 0;
float t = min_sensor_dist_;
p += t*du;
float step = min_step;
//Check if ray intersects cube
//Initially we haven't ever hit a voxel
bool hit_voxel = false;
int niter = 0;
while (t < max_sensor_dist_) {
const OctreeNode* voxel = octree_->getContainingVoxel (p (0), p(1), p(2));
if (voxel) {
hit_voxel = true;
voxel->getData (d, w);
if (((d < 0 && last_d > 0) || (d > 0 && last_d < 0)) && last_w && w) {
found_crossing = true;
float old_t = t - step;
step = (zsize_/zres_)/2;
float new_d;
float new_w;
float last_new_d = d;
float last_new_w = w;
while (t >= old_t) {
t -= step;
p -= step*du;
voxel = octree_->getContainingVoxel (p (0), p(1), p(2));
if (!voxel)
break;
voxel->getData (new_d, new_w);
if ((last_d > 0 && new_d > 0) || (last_d < 0 && new_d < 0)) {
last_d = new_d;
last_w = new_w;
d = last_new_d;
w = last_new_w;
t += step;
p += step*du;
break;
}
last_new_d = d;
last_new_w = w;
}
break;
}
last_d = d;
last_w = w;
//Update step
step = std::max (voxel->getMinSize () / 4., fabs (d)*max_dist_neg_);
}
else {
if (hit_voxel)
break;
}
t += step;
p += step*du;
niter++;
}
if (!found_crossing) {
pt.x = pt.y = pt.z =std::numeric_limits<float>::quiet_NaN();
}
else {
//Get optimal t
bool has_data = true;
float tcurr = t;
float tprev = t-step;
last_d = getTSDFValue (trans.translation ().cast<float> () + (tprev) * du,
&has_data);
d = getTSDFValue (trans.translation ().cast<float> () + tcurr * du, &has_data);
if (!has_data || pcl_isnan (d) || pcl_isnan (last_d)) {
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
}
float t_star = t + step * (-1 + fabs (last_d / (last_d -d)));
pt.getVector3fMap () = trans.translation().cast<float> () + tcurr * du;
//Get normals by looking at adjacent voxels
const OctreeNode* voxel = octree_->getContainingVoxel (pt.x, pt.y, pt.z);
if (!voxel) {
pt.normal_x = pt.normal_y = pt.normal_z = std::numeric_limits<float>::quiet_NaN();
continue;
}
float xsize, ysize, zsize;
voxel->getSize (xsize, ysize, zsize);
bool valid = true;
float d_xm = getTSDFValue (pt.x-xsize, pt.y, pt.z, &valid);
float d_xp = getTSDFValue (pt.x+xsize, pt.y, pt.z, &valid);
float d_ym = getTSDFValue (pt.x, pt.y-ysize, pt.z, &valid);
float d_yp = getTSDFValue (pt.x, pt.y+ysize, pt.z, &valid);
float d_zm = getTSDFValue (pt.x, pt.y, pt.z-zsize, &valid);
float d_zp = getTSDFValue (pt.x, pt.y, pt.z+zsize, &valid);
if (!valid) {
pt.normal_x = pt.normal_y = pt.normal_z = std::numeric_limits<float>::quiet_NaN();
continue;
}
Eigen::Vector3f dF;
dF (0) = (d_xp - d_xm)*max_dist_neg_/ (2*xsize);
dF (1) = (d_yp - d_ym)*max_dist_neg_/ (2*ysize);
dF (2) = (d_zp - d_zm)*max_dist_neg_/ (2*zsize);
dF.normalize();
pt.normal_x = dF(0);
pt.normal_y = dF(1);
pt.normal_z = dF(2);
}
}
pcl::transformPointCloudWithNormals (*cloud, *cloud, trans.inverse());
return (cloud);
}
