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; } }