void CartesianPositionController::starting(const ros::Time& time)
{
joint_command_lock_ = false;
pose_desired_ = getPose();
last_time_ = time;
loop_count_ = 0;
// reset pid controllers
for (unsigned int i=0; i<num_joints_; i++)
pid_controller_[i].reset();
ROS_INFO_STREAM("CartesianPositionController: Initial Cartesian Position = "
<< pose_desired_.p.x() <<" "
<< pose_desired_.p.y() <<" "
<< pose_desired_.p.z());
double roll, pitch, yaw;
pose_desired_.M.GetRPY(roll,pitch,yaw);
ROS_INFO_STREAM("CartesianPositionController: Initial Cartesian Rotation = roll: " << roll <<" pitch: " << pitch <<" yaw: " << yaw);
ROS_INFO_STREAM("CartesianPositionController: Initial Position = "
<< joint_positions_(0) <<" "
<< joint_positions_(1) <<" "
<< joint_positions_(2) <<" "
<< joint_positions_(3) <<" "
<< joint_positions_(4) <<" "
<< joint_positions_(5));
ROS_INFO_STREAM("CartesianPositionController: Initial Velocity = "
<< joint_velocities_(0) <<" "
<< joint_velocities_(1) <<" "
<< joint_velocities_(2) <<" "
<< joint_velocities_(3) <<" "
<< joint_velocities_(4) <<" "
<< joint_velocities_(5));
}
bool VisibilityChecker::targetIsVisible(TargetStorage& target)
{
if (map_.data.size() == 0)
{
// If no map is available, assume target is visible
return true;
}
geometry_msgs::PoseStamped robot_pose = getPose( //
"/map", robot_frame_id_, tf_listener_);
float & res = map_.info.resolution;
geometry_msgs::Point & origin = map_.info.origin.position;
// Robot
geometry_msgs::Point & pos = robot_pose.pose.position;
int rx = getIndex(origin.x, res, pos.x);
int ry = getIndex(origin.y, res, pos.y);
// Target
geometry_msgs::Pose target_pose = target.getPose();
int tx = getIndex(origin.x, res, target_pose.position.x);
int ty = getIndex(origin.y, res, target_pose.position.y);
return not raytrace::isObscured(map_, ry, rx, ty, tx);
}
/**
* @brief Moves MyPlayer
*
* @param displacement the liner movement of the player, bounded by [-0.1, 1]
* @param turn_angle the turn angle of the player, bounded by [-M_PI/30, M_PI/30]
*/
void move(double displacement, double turn_angle)
{
//Put arguments withing authorized boundaries
double max_d = 1;
displacement = (displacement > max_d ? max_d : displacement);
double min_d = -0.1;
displacement = (displacement < min_d ? min_d : displacement);
double max_t = (M_PI/30);
if (turn_angle > max_t)
turn_angle = max_t;
else if (turn_angle < -max_t)
turn_angle = -max_t;
//Compute the new reference frame
tf::Transform t_mov;
t_mov.setOrigin( tf::Vector3(displacement , 0, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, turn_angle);
t_mov.setRotation(q);
tf::Transform t = getPose();
t = t * t_mov;
//Send the new transform to ROS
br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "/map", name));
}
void Kitten::writePoseFile(char const *path) const {
// Get the current pose.
Pose pose = getPose();
// Try to open the file.
std::ofstream outFile;
outFile.open(path);
if (!outFile.good()) {
std::cerr << "could not open '" << path << "' for writing" << std::endl;
return;
}
// Write position.
outFile << pose.x_ << '\t';
outFile << pose.y_ << '\t';
outFile << pose.z_ << '\t';
// Write angles.
for (int i = 0; i < pose.angles_.size(); i++) {
outFile << pose.angles_[i] << '\t';
}
// All done!
outFile.close();
}
void CartesianPositionController::update(const ros::Time& time, const ros::Duration& period)
{
// Get time
ros::Duration dt = period;
last_time_ = time;
// Get last pose
pose_measured_ = getPose();
if(joint_command_lock_ == true){
ROS_DEBUG_STREAM("CartesianPositionController: Desired Cartesian Position = "
<< pose_desired_.p.x() <<" "
<< pose_desired_.p.y() <<" "
<< pose_desired_.p.z());
// Calculate desired joint positions from desired cartesian pos
int e = ik_solver_->CartToJnt(joint_positions_,pose_desired_,joint_positions_desired_);
joint_command_lock_ = false;
}
// Calculate the position error
KDL::JntArray joint_positions_error;
KDL::Subtract(joint_positions_,joint_positions_desired_,joint_positions_error);
// For each joint, calculate the required velocity to move to new position
for(unsigned int i=0;i<num_joints_;i++){
// Perform PID Update of Velocity
joint_velocities_command_(i) = pid_controller_[i].updatePid(joint_positions_error(i), dt);
// Calculate Instantaneous Acceleration
joint_accelerations_(i) = joint_velocities_command_(i) - joint_velocities_(i);
}
// Check acceleration with acceleration limits, and override if necessary
for(unsigned int i=0;i<num_joints_;i++){
if(fabs(joint_accelerations_(i)) > joint_acceleration_limits_[i]){
if(joint_accelerations_(i) > 0)
joint_velocities_command_(i) = joint_velocities_(i) + joint_acceleration_limits_[i];
else
joint_velocities_command_(i) = joint_velocities_(i) - joint_acceleration_limits_[i];
}
}
// Assign velocity to each joint from command
for(unsigned int i=0;i<num_joints_;i++){
// Get Joint Handle
hardware_interface::JointHandle joint = joint_handles_[i];
// Get current joint's velocity limits
double vel_limit = joint_velocity_limits_[i];
// Check command velocity agains limits
if(joint_velocities_command_(i) > vel_limit){
joint_velocities_command_(i) = vel_limit;
}else if(joint_velocities_command_(i) < -vel_limit){
joint_velocities_command_(i) = -vel_limit;
}
// Set velocity command to current joint
joint.setCommand(joint_velocities_command_(i));
}
}
void OcudumpBase::getPoseAnimated()
{
getPose();
for (Animate::iterator it=animate.begin();it!=animate.end();it++)
{
pose[it->first]+=it->second.getElem();
}
}
void CTemporalFilter::computeWeightedPose()
{
mWPose.setZero();
for(auto itr=mPoseList.begin();itr!=mPoseList.end();++itr)
mWPose+=(itr->getPose())*(itr->getWeight());
mWPose/=mPoseList.size();
//mPoseList.sort(cmpW);
}
osg::Vec3 Primitive::toLocal(const osg::Vec3& pos) const {
const Pose& m = Pose::inverse(getPose());
return pos*m;
// osg::Vec4 p(pos,1);
// const osg::Vec4& pl = p*m;
// // one should only use the transpose here, but osg does not have it!
// return osg::Vec3(pl.x(),pl.y(), pl.z());
}
void ScanMatcher::getPose(geometry_msgs::PoseWithCovarianceStamped& pose) const {
pose.header.stamp = transform_.stamp_;
pose.header.frame_id = transform_.frame_id_;
getPose(pose.pose.pose);
if (covariance_valid_)
std::copy(&(covariance_(0,0)), &(covariance_(5,5)) + 1, pose.pose.covariance.begin());
else
pose.pose.covariance.assign(0.0);
}
bool leatherman::getFrame(const arm_navigation_msgs::MultiDOFJointState &state, std::string frame_id, std::string child_frame_id, KDL::Frame &frame)
{
geometry_msgs::Pose p;
if(!getPose(state, frame_id, child_frame_id, p))
return false;
Eigen::Affine3d t;
leatherman::poseFromMsg(p, t);
leatherman::transformEigenToKDL(t, frame);
return true;
}
Matrix BaseActor::getWorldPose() const
{
Matrix worldPose = getPose();
const BaseActor* actor = this;
while (actor = actor->m_parent)
{
worldPose *= actor->getPose();
}
return worldPose;
}
bool Primitive::store(FILE* f) const {
const Pose& pose = getPose();
const Pos& vel = getVel();
const Pos& avel = getAngularVel();
if ( fwrite ( pose.ptr() , sizeof ( Pose::value_type), 16, f ) == 16 )
if( fwrite ( vel.ptr() , sizeof ( Pos::value_type), 3, f ) == 3 )
if( fwrite ( avel.ptr() , sizeof ( Pos::value_type), 3, f ) == 3 )
return true;
return false;
}
void CTemporalFilter::reduceNNN(size_t pNNN)
{
if(pNNN>mPoseList.size())
return;
//mPoseList.sort(cmpW);
auto itr=mPoseList.begin();
advance(itr,pNNN);
mPoseList.erase(itr,mPoseList.end());
mWPose.setZero();
for(auto itr=mPoseList.begin();itr!=mPoseList.end();++itr)
mWPose+=itr->getPose()*itr->getWeight();
mWPose/=mPoseList.size();
}
Matrix4f RobotLink::getPoseAsGpuMat4f()
{
// call getPose() explicitely and do NOT use pose_property_
// to trigger recursive calculation
KDL::Frame pose = getPose();
Matrix4f transformation(
pose.M.data[0], pose.M.data[1], pose.M.data[2], pose.p.x(),
pose.M.data[3], pose.M.data[4], pose.M.data[5], pose.p.y(),
pose.M.data[6], pose.M.data[7], pose.M.data[8], pose.p.z(),
0.0, 0.0, 0.0, 1.0
);
return transformation;
}
float InputDevice::getValue(ChannelType channelType, uint16_t channel) const {
switch (channelType) {
case ChannelType::AXIS:
return getAxis(channel);
case ChannelType::BUTTON:
return getButton(channel);
case ChannelType::POSE:
return getPose(channel).valid ? 1.0f : 0.0f;
default:
break;
}
return 0.0f;
}
void OsgRepresentation::update(double dt)
{
if (isActive())
{
std::pair<osg::Quat, osg::Vec3d> pose = toOsg(getPose());
m_transform->setAttitude(pose.first);
m_transform->setPosition(pose.second);
doUpdate(dt);
setVisible(true);
}
else
{
setVisible(false);
}
}
void Mesh::init(const OdeHandle& odeHandle, double mass, const OsgHandle& osgHandle,
char mode) {
// 20100307; guettler: sometimes the Geom is created later (XMLBoundingShape),
// if no body is created, this Mesh seems to be static. Then the BoundingShape must not attach
// any Primitive to the body of the Mesh by a Transform.
//assert(mode & Body || mode & Geom);
if (!substanceManuallySet)
substance = odeHandle.substance;
this->mode=mode;
double r=0.01;
QMP_CRITICAL(7);
if (mode & Draw){
osgmesh->init(osgHandle);
r = osgmesh->getRadius();
}
else {
osgmesh->virtualInit(osgHandle);
}
if (r<0) r=0.01;
if (mode & Body){
body = dBodyCreate (odeHandle.world);
// Todo: use compound bounding box mass instead
setMass(mass, mode & Density);
}
// read boundingshape file
// const osg::BoundingSphere& bsphere = osgmesh->getGroup()->getBound();
// 20100307; guettler: if no Geom, don't create any Geom or Boundings (this is used e.g. for Meshes loaded from XML)
if (mode & Geom) {
short drawBoundingMode;
if (osgHandle.drawBoundings)
drawBoundingMode=Primitive::Geom | Primitive::Draw;
else
drawBoundingMode=Primitive::Geom;
boundshape = new BoundingShape(filename+".bbox" ,this);
if(!boundshape->init(odeHandle, osgHandle.changeColor(Color(1,0,0,0.3)), scale, drawBoundingMode)){
printf("use default bounding box, because bbox file not found!\n");
Primitive* bound = new Sphere(r);
Transform* trans = new Transform(this,bound,Pose::translate(0.0f,0.0f,0.0f));
trans->init(odeHandle, 0, osgHandle.changeColor(Color(1,0,0,0.3)),drawBoundingMode);
osgmesh->setMatrix(Pose::translate(0.0f,0.0f,osgmesh->getRadius())*getPose()); // set obstacle higher
}
}
QMP_END_CRITICAL(7);
}
void
v4r::Registration::MultiSessionModelling<PointT>::computeCost(EdgeBetweenPartialModels & edge)
{
//fsv computation
//take all views of edge_i with respective indices and compute FSV with respect to the views of j_
//do i need normals?
float fsv = 0.f;
int non_valid = 0;
int total = 0;
for(size_t ii=session_ranges_[edge.i_].first; ii <= session_ranges_[edge.i_].second; ii++)
{
typename pcl::PointCloud<PointT>::ConstPtr cloud = clouds_[ii];
std::vector<int> & indices = getIndices(ii);
Eigen::Matrix4f pose = edge.transformation_.inverse() * getPose(ii);
for(size_t jj=session_ranges_[edge.j_].first; jj <= session_ranges_[edge.j_].second; jj++)
{
Eigen::Matrix4f pose_to_jj = poses_[jj].inverse() * pose;
float fsv_loc = computeFSV(cloud, normals_[ii], indices, pose_to_jj, clouds_[jj]);
if(fsv_loc < 0)
{
non_valid++;
}
else
{
fsv += fsv_loc;
}
total++;
}
}
if(non_valid == total)
{
std::cout << "No valid edge..." << std::endl;
edge.cost_ = std::numeric_limits<float>::infinity();
}
else
{
edge.cost_ = fsv / (total - non_valid);
}
}
//-----------------------------------------------------------------------------
// LLKeyframeStandMotion::onInitialize()
//-----------------------------------------------------------------------------
LLMotion::LLMotionInitStatus LLKeyframeStandMotion::onInitialize(LLCharacter *character)
{
// save character pointer for later use
mCharacter = character;
mFlipFeet = FALSE;
// load keyframe data, setup pose and joint states
LLMotion::LLMotionInitStatus status = LLKeyframeMotion::onInitialize(character);
if ( status == STATUS_FAILURE )
{
return status;
}
// find the necessary joint states
LLPose *pose = getPose();
mPelvisState = pose->findJointState("mPelvis");
mHipLeftState = pose->findJointState("mHipLeft");
mKneeLeftState = pose->findJointState("mKneeLeft");
mAnkleLeftState = pose->findJointState("mAnkleLeft");
mHipRightState = pose->findJointState("mHipRight");
mKneeRightState = pose->findJointState("mKneeRight");
mAnkleRightState = pose->findJointState("mAnkleRight");
if ( !mPelvisState ||
!mHipLeftState ||
!mKneeLeftState ||
!mAnkleLeftState ||
!mHipRightState ||
!mKneeRightState ||
!mAnkleRightState )
{
llinfos << getName() << ": Can't find necessary joint states" << llendl;
return STATUS_FAILURE;
}
return STATUS_SUCCESS;
}
void ppfmap::CudaPPFMatch<PointT, NormalT>::detect(
const PointCloudPtr cloud,
const NormalsPtr normals,
std::vector<Pose>& poses) {
float affine_s[12];
std::vector<Pose> pose_vector;
const float radius = map->getCloudDiameter() * neighborhood_percentage;
std::vector<int> indices;
std::vector<float> distances;
pcl::KdTreeFLANN<PointT> kdtree;
kdtree.setInputCloud(cloud);
if (!use_indices) {
ref_point_indices->resize(cloud->size());
for (int i = 0; i < cloud->size(); i++) {
(*ref_point_indices)[i] = i;
}
}
poses.clear();
for (const auto index : *ref_point_indices) {
const auto& point = cloud->at(index);
const auto& normal = normals->at(index);
if (!pcl::isFinite(point)) continue;
getAlignmentToX(point, normal, &affine_s);
kdtree.radiusSearch(point, radius, indices, distances);
poses.push_back(getPose(index, indices, cloud, normals, affine_s));
}
sort(poses.begin(), poses.end(),
[](const Pose& a, const Pose& b) -> bool {
return a.votes > b.votes;
});
}
template<typename PointT> inline Eigen::Affine3f
pcl::registration::LUM<PointT>::getTransformation (Vertex vertex)
{
Eigen::Vector6f pose = getPose (vertex);
return (pcl::getTransformation (pose (0), pose (1), pose (2), pose (3), pose (4), pose (5)));
}
void UserInputMapper::update(float deltaTime) {
// Reset the axis state for next loop
for (auto& channel : _actionStates) {
channel = 0.0f;
}
for (auto& channel : _poseStates) {
channel = PoseValue();
}
int currentTimestamp = 0;
for (auto& channelInput : _actionToInputsMap) {
auto& inputMapping = channelInput.second;
auto& inputID = inputMapping._input;
bool enabled = true;
// Check if this input channel has modifiers and collect the possibilities
auto modifiersIt = _inputToModifiersMap.find(inputID.getID());
if (modifiersIt != _inputToModifiersMap.end()) {
Modifiers validModifiers;
bool isActiveModifier = false;
for (auto& modifier : modifiersIt->second) {
auto deviceProxy = getDeviceProxy(modifier);
if (deviceProxy->getButton(modifier, currentTimestamp)) {
validModifiers.push_back(modifier);
isActiveModifier |= (modifier.getID() == inputMapping._modifier.getID());
}
}
enabled = (validModifiers.empty() && !inputMapping.hasModifier()) || isActiveModifier;
}
// if enabled: default input or all modifiers on
if (enabled) {
auto deviceProxy = getDeviceProxy(inputID);
switch (inputMapping._input.getType()) {
case ChannelType::BUTTON: {
_actionStates[channelInput.first] += inputMapping._scale * float(deviceProxy->getButton(inputID, currentTimestamp));// * deltaTime; // weight the impulse by the deltaTime
break;
}
case ChannelType::AXIS: {
_actionStates[channelInput.first] += inputMapping._scale * deviceProxy->getAxis(inputID, currentTimestamp);
break;
}
case ChannelType::POSE: {
if (!_poseStates[channelInput.first].isValid()) {
_poseStates[channelInput.first] = deviceProxy->getPose(inputID, currentTimestamp);
}
break;
}
default: {
break; //silence please
}
}
} else{
// Channel input not enabled
enabled = false;
}
}
// Scale all the channel step with the scale
static const float EPSILON = 0.01f;
for (auto i = 0; i < NUM_ACTIONS; i++) {
_actionStates[i] *= _actionScales[i];
// Emit only on change, and emit when moving back to 0
if (fabs(_actionStates[i] - _lastActionStates[i]) > EPSILON) {
_lastActionStates[i] = _actionStates[i];
emit actionEvent(i, _actionStates[i]);
}
// TODO: emit signal for pose changes
}
}
void
pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::recognize ()
{
models_.reset (new std::vector<ModelT>);
transforms_.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
PointInTPtr processed;
typename pcl::PointCloud<FeatureT>::Ptr signatures (new pcl::PointCloud<FeatureT> ());
//pcl::PointCloud<int> keypoints_input;
PointInTPtr keypoints_pointcloud;
if (signatures_ != 0 && processed_ != 0 && (signatures_->size () == keypoints_pointcloud->points.size ()))
{
keypoints_pointcloud = keypoints_input_;
signatures = signatures_;
processed = processed_;
std::cout << "Using the ISPK ..." << std::endl;
}
else
{
processed.reset( (new pcl::PointCloud<PointInT>));
if (indices_.size () > 0)
{
PointInTPtr sub_input (new pcl::PointCloud<PointInT>);
pcl::copyPointCloud (*input_, indices_, *sub_input);
estimator_->estimate (sub_input, processed, keypoints_pointcloud, signatures);
}
else
{
estimator_->estimate (input_, processed, keypoints_pointcloud, signatures);
}
processed_ = processed;
}
std::cout << "Number of keypoints:" << keypoints_pointcloud->points.size () << std::endl;
int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float);
//feature matching and object hypotheses
std::map<std::string, ObjectHypothesis> object_hypotheses;
{
for (size_t idx = 0; idx < signatures->points.size (); idx++)
{
float* hist = signatures->points[idx].histogram;
std::vector<float> std_hist (hist, hist + size_feat);
flann_model histogram;
histogram.descr = std_hist;
flann::Matrix<int> indices;
flann::Matrix<float> distances;
nearestKSearch (flann_index_, histogram, 1, indices, distances);
//read view pose and keypoint coordinates, transform keypoint coordinates to model coordinates
Eigen::Matrix4f homMatrixPose;
getPose (flann_models_.at (indices[0][0]).model, flann_models_.at (indices[0][0]).view_id, homMatrixPose);
typename pcl::PointCloud<PointInT>::Ptr keypoints (new pcl::PointCloud<PointInT> ());
getKeypoints (flann_models_.at (indices[0][0]).model, flann_models_.at (indices[0][0]).view_id, keypoints);
PointInT view_keypoint = keypoints->points[flann_models_.at (indices[0][0]).keypoint_id];
PointInT model_keypoint;
model_keypoint.getVector4fMap () = homMatrixPose.inverse () * view_keypoint.getVector4fMap ();
typename std::map<std::string, ObjectHypothesis>::iterator it_map;
if ((it_map = object_hypotheses.find (flann_models_.at (indices[0][0]).model.id_)) != object_hypotheses.end ())
{
//if the object hypothesis already exists, then add information
ObjectHypothesis oh = (*it_map).second;
oh.correspondences_pointcloud->points.push_back (model_keypoint);
oh.correspondences_to_inputcloud->push_back (
pcl::Correspondence (static_cast<int> (oh.correspondences_pointcloud->points.size () - 1),
static_cast<int> (idx), distances[0][0]));
oh.feature_distances_->push_back (distances[0][0]);
}
else
{
//create object hypothesis
ObjectHypothesis oh;
typename pcl::PointCloud<PointInT>::Ptr correspondences_pointcloud (new pcl::PointCloud<PointInT> ());
correspondences_pointcloud->points.push_back (model_keypoint);
oh.model_ = flann_models_.at (indices[0][0]).model;
oh.correspondences_pointcloud = correspondences_pointcloud;
//last keypoint for this model is a correspondence the current scene keypoint
pcl::CorrespondencesPtr corr (new pcl::Correspondences ());
oh.correspondences_to_inputcloud = corr;
oh.correspondences_to_inputcloud->push_back (pcl::Correspondence (0, static_cast<int> (idx), distances[0][0]));
boost::shared_ptr < std::vector<float> > feat_dist (new std::vector<float>);
feat_dist->push_back (distances[0][0]);
oh.feature_distances_ = feat_dist;
object_hypotheses[oh.model_.id_] = oh;
}
}
}
typename std::map<std::string, ObjectHypothesis>::iterator it_map;
std::vector<float> feature_distance_avg;
{
//pcl::ScopeTime t("Geometric verification, RANSAC and transform estimation");
for (it_map = object_hypotheses.begin (); it_map != object_hypotheses.end (); it_map++)
{
std::vector < pcl::Correspondences > corresp_clusters;
cg_algorithm_->setSceneCloud (keypoints_pointcloud);
cg_algorithm_->setInputCloud ((*it_map).second.correspondences_pointcloud);
cg_algorithm_->setModelSceneCorrespondences ((*it_map).second.correspondences_to_inputcloud);
cg_algorithm_->cluster (corresp_clusters);
std::cout << "Instances:" << corresp_clusters.size () << " Total correspondences:" << (*it_map).second.correspondences_to_inputcloud->size () << " " << it_map->first << std::endl;
std::vector<bool> good_indices_for_hypothesis (corresp_clusters.size (), true);
if (threshold_accept_model_hypothesis_ < 1.f)
{
//sort the hypotheses for each model according to their correspondences and take those that are threshold_accept_model_hypothesis_ over the max cardinality
int max_cardinality = -1;
for (size_t i = 0; i < corresp_clusters.size (); i++)
{
//std::cout << (corresp_clusters[i]).size() << " -- " << (*(*it_map).second.correspondences_to_inputcloud).size() << std::endl;
if (max_cardinality < static_cast<int> (corresp_clusters[i].size ()))
{
max_cardinality = static_cast<int> (corresp_clusters[i].size ());
}
}
for (size_t i = 0; i < corresp_clusters.size (); i++)
{
if (static_cast<float> ((corresp_clusters[i]).size ()) < (threshold_accept_model_hypothesis_ * static_cast<float> (max_cardinality)))
{
good_indices_for_hypothesis[i] = false;
}
}
}
for (size_t i = 0; i < corresp_clusters.size (); i++)
{
if (!good_indices_for_hypothesis[i])
continue;
//drawCorrespondences (processed, it_map->second, keypoints_pointcloud, corresp_clusters[i]);
Eigen::Matrix4f best_trans;
typename pcl::registration::TransformationEstimationSVD < PointInT, PointInT > t_est;
t_est.estimateRigidTransformation (*(*it_map).second.correspondences_pointcloud, *keypoints_pointcloud, corresp_clusters[i], best_trans);
models_->push_back ((*it_map).second.model_);
transforms_->push_back (best_trans);
}
}
}
std::cout << "Number of hypotheses:" << models_->size() << std::endl;
/**
* POSE REFINEMENT
**/
if (ICP_iterations_ > 0)
{
pcl::ScopeTime ticp ("ICP ");
//Prepare scene and model clouds for the pose refinement step
PointInTPtr cloud_voxelized_icp (new pcl::PointCloud<PointInT> ());
pcl::VoxelGrid<PointInT> voxel_grid_icp;
voxel_grid_icp.setInputCloud (processed);
voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_);
voxel_grid_icp.filter (*cloud_voxelized_icp);
source_->voxelizeAllModels (VOXEL_SIZE_ICP_);
#pragma omp parallel for schedule(dynamic,1) num_threads(omp_get_num_procs())
for (int i = 0; i < static_cast<int>(models_->size ()); i++)
{
ConstPointInTPtr model_cloud;
PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_);
pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
typename pcl::registration::CorrespondenceRejectorSampleConsensus<PointInT>::Ptr rej (
new pcl::registration::CorrespondenceRejectorSampleConsensus<PointInT> ());
rej->setInputTarget (cloud_voxelized_icp);
rej->setMaximumIterations (1000);
rej->setInlierThreshold (0.005f);
rej->setInputSource (model_aligned);
pcl::IterativeClosestPoint<PointInT, PointInT> reg;
reg.addCorrespondenceRejector (rej);
reg.setInputTarget (cloud_voxelized_icp); //scene
reg.setInputSource (model_aligned); //model
reg.setMaximumIterations (ICP_iterations_);
reg.setMaxCorrespondenceDistance (VOXEL_SIZE_ICP_ * 4.f);
typename pcl::PointCloud<PointInT>::Ptr output_ (new pcl::PointCloud<PointInT> ());
reg.align (*output_);
Eigen::Matrix4f icp_trans = reg.getFinalTransformation ();
transforms_->at (i) = icp_trans * transforms_->at (i);
}
}
/**
* HYPOTHESES VERIFICATION
**/
if (hv_algorithm_)
{
pcl::ScopeTime thv ("HV verification");
std::vector<typename pcl::PointCloud<PointInT>::ConstPtr> aligned_models;
aligned_models.resize (models_->size ());
for (size_t i = 0; i < models_->size (); i++)
{
ConstPointInTPtr model_cloud = models_->at (i).getAssembled (0.0025f);
//ConstPointInTPtr model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_);
PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
aligned_models[i] = model_aligned;
}
std::vector<bool> mask_hv;
hv_algorithm_->setSceneCloud (input_);
hv_algorithm_->addModels (aligned_models, true);
hv_algorithm_->verify ();
hv_algorithm_->getMask (mask_hv);
boost::shared_ptr < std::vector<ModelT> > models_temp;
boost::shared_ptr < std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > > transforms_temp;
models_temp.reset (new std::vector<ModelT>);
transforms_temp.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
for (size_t i = 0; i < models_->size (); i++)
{
if (!mask_hv[i])
continue;
models_temp->push_back (models_->at (i));
transforms_temp->push_back (transforms_->at (i));
}
models_ = models_temp;
transforms_ = transforms_temp;
}
}
void OcudumpDebug::getPoseAnimated()
{
getPose();
}
AREXPORT void ArArg::log(void) const
{
std::list<ArArgumentBuilder *>::const_iterator it;
const std::list<ArArgumentBuilder *> *argList;
switch (getType())
{
case ArArg::INVALID:
ArLog::log(ArLog::Terse,
"\tType: %10s. This argument was not created properly.",
"invalid");
case ArArg::INT:
ArLog::log(ArLog::Terse, "\tType: %10s name: %12s value: %d", "int",
getName(), getInt());
if (strlen(getDescription()) != 0)
ArLog::log(ArLog::Terse, "\t\tDescription: %s",
getDescription());
break;
case ArArg::DOUBLE:
ArLog::log(ArLog::Terse, "\tType: %10s name: %12s value: %f", "double",
getName(), getDouble());
if (strlen(getDescription()) != 0)
ArLog::log(ArLog::Terse, "\t\tDescription: %s",
getDescription());
break;
case ArArg::STRING:
ArLog::log(ArLog::Terse, "\tType: %10s name: %12s value: %s", "string",
getName(), getString());
if (strlen(getDescription()) != 0)
ArLog::log(ArLog::Terse, "\t\tDescription: %s",
getDescription());
break;
case ArArg::BOOL:
ArLog::log(ArLog::Terse, "\tType: %10s name: %12s value: %d", "bool",
getName(), getBool());
if (strlen(getDescription()) != 0)
ArLog::log(ArLog::Terse, "\t\tDescription: %s",
getDescription());
break;
case ArArg::POSE:
ArLog::log(ArLog::Terse, "\tType: %10s name: %12s value: (%.1f %.1f %.1f)",
"pose", getName(), getPose().getX(), getPose().getY(),
getPose().getTh());
if (strlen(getDescription()) != 0)
ArLog::log(ArLog::Terse, "\t\tDescription: %s",
getDescription());
break;
case ArArg::FUNCTOR:
ArLog::log(ArLog::Terse, "\tType: %10s name: %12s",
"functor", getName(), getPose().getX(), getPose().getY(),
getPose().getTh());
if (strlen(getDescription()) != 0)
ArLog::log(ArLog::Terse, "\t\tDescription: %s",
getDescription());
ArLog::log(ArLog::Terse, "\t\tValues:");
argList = myGetFunctor->invokeR();
for (it = argList->begin(); it != argList->end(); it++)
ArLog::log(ArLog::Terse, "\t\t\t%s", (*it)->getFullString());
break;
case ArArg::DESCRIPTION_HOLDER:
ArLog::log(ArLog::Terse, "\tType: %20s Description: %s",
"description_holder", getDescription());
default:
ArLog::log(ArLog::Terse,
"\tType: %10s. This type doesn't have a case in ArArg::print.",
"unknown");
break;
}
if (myConfigPrioritySet)
ArLog::log(ArLog::Terse, "\t\tPriority: %s",
ArPriority::getPriorityName(myConfigPriority));
}
void ScanMatcher::getPose(geometry_msgs::PoseStamped& pose) const {
pose.header.stamp = transform_.stamp_;
pose.header.frame_id = transform_.frame_id_;
getPose(pose.pose);
}
geometry_msgs::PoseStamped ScanMatcher::getPose() const {
geometry_msgs::PoseStamped pose;
getPose(pose);
return pose;
}
geometry_msgs::PoseWithCovarianceStamped ScanMatcher::getPoseWithCovariance() const {
geometry_msgs::PoseWithCovarianceStamped pose_with_covariance;
getPose(pose_with_covariance);
return pose_with_covariance;
}
template<typename PointT> void
pcl::registration::LUM<PointT>::compute ()
{
int n = static_cast<int> (getNumVertices ());
if (n < 2)
{
PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n");
return;
}
for (int i = 0; i < max_iterations_; ++i)
{
// Linearized computation of C^-1 and C^-1*D and convergence checking for all edges in the graph (results stored in slam_graph_)
typename SLAMGraph::edge_iterator e, e_end;
for (boost::tuples::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e)
computeEdge (*e);
// Declare matrices G and B
Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1));
Eigen::VectorXf B = Eigen::VectorXf::Zero (6 * (n - 1));
// Start at 1 because 0 is the reference pose
for (int vi = 1; vi != n; ++vi)
{
for (int vj = 0; vj != n; ++vj)
{
// Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge
Edge e;
bool present1, present2;
boost::tuples::tie (e, present1) = edge (vi, vj, *slam_graph_);
if (!present1)
{
boost::tuples::tie (e, present2) = edge (vj, vi, *slam_graph_);
if (!present2)
continue;
}
// Fill in elements of G and B
if (vj > 0)
G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
}
}
// Computation of the linear equation system: GX = B
// TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse)
Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B);
// Update the poses
float sum = 0.0;
for (int vi = 1; vi != n; ++vi)
{
Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f> (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6));
sum += difference_pose.norm ();
setPose (vi, getPose (vi) + difference_pose);
}
// Convergence check
if (sum <= convergence_threshold_ * static_cast<float> (n - 1))
return;
}
}
void TreeOptimizer2::propagateErrors(){
iteration++;
int edgeCount=0;
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%10000)) DEBUG(1) << "c";
Edge* e=*it;
Vertex* top=e->top;
Vertex* v1=e->v1;
Vertex* v2=e->v2;
double l=e->length;
DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
Pose p1=getPose(v1, top);
Pose p2=getPose(v2, top);
DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
Transformation et=e->transformation;
Transformation t1(p1);
Transformation t2(p2);
Transformation t12=t1*et;
Pose p12=t12.toPoseType();
DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
double angle=r.theta();
angle=atan2(sin(angle),cos(angle));
r.theta()=angle;
DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
InformationMatrix S=e->informationMatrix;
InformationMatrix R;
R.values[0][0]=t1.rotationMatrix[0][0];
R.values[0][1]=t1.rotationMatrix[0][1];
R.values[0][2]=0;
R.values[1][0]=t1.rotationMatrix[1][0];
R.values[1][1]=t1.rotationMatrix[1][1];
R.values[1][2]=0;
R.values[2][0]=0;
R.values[2][1]=0;
R.values[2][2]=1;
InformationMatrix W=R*S*R.transpose();
Pose d=W*r*2.;
DEBUG(2) << " d=" << d.x() << " " << d.y() << " " << d.theta() << endl;
assert(l>0);
double alpha[3] = { 1./(gamma[0]*iteration), 1./(gamma[1]*iteration), 1./(gamma[2]*iteration) };
double tw[3]={0.,0.,0.};
for (int dir=0; dir<2; dir++) {
Vertex* n = (dir==0)? v1 : v2;
while (n!=top){
uint i=n->id;
tw[0]+=1./M[i].values[0];
tw[1]+=1./M[i].values[1];
tw[2]+=1./M[i].values[2];
n=n->parent;
}
}
double beta[3] = {l*alpha[0]*d.values[0], l*alpha[1]*d.values[1], l*alpha[2]*d.values[2]};
beta[0]=(fabs(beta[0])>fabs(r.values[0]))?r.values[0]:beta[0];
beta[1]=(fabs(beta[1])>fabs(r.values[1]))?r.values[1]:beta[1];
beta[2]=(fabs(beta[2])>fabs(r.values[2]))?r.values[2]:beta[2];
DEBUG(2) << " alpha=" << alpha[0] << " " << alpha[1] << " " << alpha[2] << endl;
DEBUG(2) << " beta=" << beta[0] << " " << beta[1] << " " << beta[2] << endl;
for (int dir=0; dir<2; dir++) {
Vertex* n = (dir==0)? v1 : v2;
double sign=(dir==0)? -1. : 1.;
while (n!=top){
uint i=n->id;
assert(M[i].values[0]>0);
assert(M[i].values[1]>0);
assert(M[i].values[2]>0);
Pose delta( beta[0]/(M[i].values[0]*tw[0]), beta[1]/(M[i].values[1]*tw[1]), beta[2]/(M[i].values[2]*tw[2]));
delta=delta*sign;
DEBUG(2) << " " << dir << ":" << i <<"," << n->parent->id << ":"
<< n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta() << " -> ";
n->parameters.x()+=delta.x();
n->parameters.y()+=delta.y();
n->parameters.theta()+=delta.theta();
DEBUG(2) << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta()<< endl;
n=n->parent;
}
}
updatePoseChain(v1,top);
updatePoseChain(v2,top);
Pose pf1=v1->pose;
Pose pf2=v2->pose;
DEBUG(2) << " pf1=" << pf1.x() << " " << pf1.y() << " " << pf1.theta() << endl;
DEBUG(2) << " pf2=" << pf2.x() << " " << pf2.y() << " " << pf2.theta() << endl;
DEBUG(2) << " en=" << p12.x()-pf2.x() << " " << p12.y()-pf2.y() << " " << p12.theta()-pf2.theta() << endl;
}
}
