void Transform::SetTransform(const Pose2& rPose1, const Pose2& rPose2) { if (rPose1 == rPose2) { m_Rotation.SetToIdentity(); m_InverseRotation.SetToIdentity(); m_Transform = Pose2(); return; } // heading transformation m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading()); m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading()); // position transformation Pose2 newPosition; if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0) { newPosition = rPose2 - m_Rotation * rPose1; } else { newPosition = rPose2; } m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading()); }
void FAnimationNode_TwoWayBlend::Evaluate(FPoseContext& Output) { const float ActualAlpha = AlphaScaleBias.ApplyTo(Alpha); if (ActualAlpha > ZERO_ANIMWEIGHT_THRESH) { if (ActualAlpha < 1.0f - ZERO_ANIMWEIGHT_THRESH) { FPoseContext Pose1(Output); FPoseContext Pose2(Output); A.Evaluate(Pose1); B.Evaluate(Pose2); FAnimationRuntime::BlendTwoPosesTogether(Pose1.Pose, Pose2.Pose, Pose1.Curve, Pose2.Curve, (1.0f - ActualAlpha), Output.Pose, Output.Curve); } else { B.Evaluate(Output); } } else { A.Evaluate(Output); } }
// refine the plane given set of inlier points gtsam::Pose2 PoseEstimator::refine(const Datums& ps, const Mask& mask, boost::optional<Model> bestModel) { // filter inlier std::vector<Point2Pair> putatives; for (size_t i = 0; i < ps.size(); i++) { if (!mask[i]) continue; putatives.push_back(ps[i]); } // check inlier size if (putatives.size() < 2) throw std::runtime_error("minimal pose solver must have input inlier size >= 2"); // method of 'A Method for Registration of 3D Shapes', by Besl and McKay, 1992 // TODO: not sure this is correct in 2D // 1. find centroids of each dataset Vector2 cent1 = zero(2); Vector2 cent2 = zero(2); for (size_t i = 0; i < putatives.size(); i++) { cent1 += putatives[i].first.vector(); cent2 += putatives[i].second.vector(); } cent1 = cent1 / static_cast<double>(putatives.size()); cent2 = cent2 / static_cast<double>(putatives.size()); // 2. SVD Matrix H = zeros(2,2); for (size_t i = 0; i < putatives.size(); i++) H = H + (putatives[i].first.vector() - cent1) * (putatives[i].second.vector() - cent1).transpose(); Matrix U,V; Vector S; svd(H, U, S, V); // 3. get rotation matrix Matrix R = V * U.transpose(); if (R.determinant() < 0) { V(0,1) = -V(0,1); V(1,1) = -V(1,1); R = V * U.transpose(); } // 4. translation Vector2 t = -R * cent1 + cent2; return Pose2(Rot2(atan2(R(1,0), R(0,0))), Point2(t)); }
gtsam::NonlinearFactorGraph Matcher2D::findLocalLoopClosure( const PoseD slam_pose, LaserScan2D& scan) { NonlinearFactorGraph graph; #if 0 // get looped index vector<LoopResult2d> loop_result; loop_result = this->findLoopClosure(scan); // perform small EM only after init if (local_smallEM_.flag_init) { for (size_t i = 0; i < loop_result.size(); i++) { Pose2 relpose = loop_result[i].delta_pose; pair<size_t, size_t> relidx = make_pair(loop_result[i].loop_idx, pose_count_); // inlier if (pose_count_ - loop_result[i].loop_idx > setting_.local_loop_interval && local_smallEM_.perform(relpose, relidx, curr_values_, isam_.getFactorsUnsafe())) { cout << "local loop detected! " << endl; cout << "robot_" << ID_ << ": [" << loop_result[i].loop_idx << ", " << pose_count_ << "]" << endl; cout << "Press Enter to continue ... " << endl; cin.ignore(1); // matched: insert between robot factor graph.push_back(BetweenFactor<Pose2>(Symbol(ID_, loop_result[i].loop_idx), Symbol(ID_, pose_count_), relpose, setting_.loop_default_model)); } } } else { // only init small EM after certain count if (local_measure_poses_.size() >= setting_.local_loop_count_smallEM) { local_smallEM_.init(local_measure_poses_, local_measure_index_, Pose2()); local_measure_poses_.clear(); local_measure_index_.clear(); // insert in local cache } else { for (size_t i = 0; i < loop_result.size(); i++) { local_measure_poses_.push_back(loop_result[i].delta_pose); local_measure_index_.push_back(make_pair(loop_result[i].loop_idx, pose_count_)); } } } #endif return graph; }
Transform::Transform(const Pose2& rPose) { SetTransform(Pose2(), rPose); }
Transform::Transform() { SetTransform(Pose2(), Pose2()); }
Sensor::Sensor(const Name& rName) : Object(rName) { m_pOffsetPose = new Parameter<Pose2>("OffsetPose", Pose2(), GetParameterManager()); }
// compute covariance, using input format from errorminizer Eigen::Matrix3d ICPMatcher::estimateCovariance( const PMatcher::DataPoints& reading, const PMatcher::DataPoints& reference, const PMatcher::Matches& matches, const PMatcher::OutlierWeights& outlierWeights, const PMatcher::TransformationParameters& trans) { // prepare vars size_t max_nr_point = outlierWeights.cols(); Eigen::Matrix3d d2J_dx2 = Eigen::MatrixXd::Zero(3,3); // Hessian Eigen::MatrixXd d2J_dxdz_reading = Eigen::MatrixXd::Zero(3,max_nr_point); // Jacobian Eigen::MatrixXd d2J_dxdz_reference = Eigen::MatrixXd::Zero(3,max_nr_point); // Jacobian Eigen::Vector2d reading_point(2); Eigen::Vector2d reference_point(2); Eigen::Vector2d normal(2); Eigen::Vector2d reading_direction(2); Eigen::Vector2d reference_direction(2); // normals of ref Eigen::MatrixXd normals = reference.getDescriptorCopyByName("normals"); if (normals.rows() < 2) // Make sure there are normals in DataPoints throw std::runtime_error("[ERROR:ICPMatcher] normals of reference not exist"); Pose2 trans_pose = Pose2(trans); double theta = trans_pose.theta(); double x = trans_pose.x(); double y = trans_pose.y(); size_t valid_points_count = 0; // calculate each point for (size_t i = 0; i < max_nr_point; ++i) { // only select inlier point if (outlierWeights(0, i) > 0.0) { // prepare data reading_point = reading.features.block<2, 1>(0, i); //cout << reading_point << endl; int reference_idx = matches.ids(0, i); reference_point = reference.features.block<2, 1>(0, reference_idx); //cout << reference_point << endl; normal = normals.block<2, 1>(0, reference_idx); //cout << normal << endl; //normal = normal/normal.norm(); double reading_range = reading_point.norm(); reading_direction = reading_point / reading_range; double reference_range = reference_point.norm(); reference_direction = reference_point / reference_range; // the covariance paper gives the following: // cov(x) = inv(d2J_dx2) * d2J_dxdz * cov(z) * d2J_dxdz' * inv(d2J_dx2) // cov(z) is diagonal so process later Eigen::Matrix3d tmp_mat3(3,3); Eigen::Vector3d tmp_vec3(3); // 1. d2J_dx2 tmp_mat3(0,0) = 2.0 * normal(0)*normal(0); tmp_mat3(1,1) = 2.0 * normal(1)*normal(1); tmp_mat3(1,0) = 2.0 * normal(0)*normal(1); tmp_mat3(0,1) = tmp_mat3(1,0); double a_cvx_a_svy = cos(reference_direction(0)) + sin(reference_direction(1)); double m_svx_a_cvy = -sin(reference_direction(0)) + cos(reference_direction(1)); double tmp1 = reference_range * (normal(1)*m_svx_a_cvy + normal(0)*a_cvx_a_svy); tmp_mat3(2,0) = 2.0 * normal(0) * tmp1; tmp_mat3(2,1) = 2.0 * normal(1) * tmp1; tmp_mat3(0,2) = tmp_mat3(2,0); tmp_mat3(1,2) = tmp_mat3(2,1); tmp_mat3(2,2) = 2.0 * tmp1 * tmp1; d2J_dx2 += tmp_mat3; // d2J_dxdz_reading tmp1 = -normal(0)*reading_direction(0) - normal(1)*reading_direction(1); tmp_vec3(0) = 2.0 * normal(0) * tmp1; tmp_vec3(1) = 2.0 * normal(1) * tmp1; tmp_vec3(2) = 2.0 * tmp1 *reference_range * (normal(1)*m_svx_a_cvy + normal(0)*a_cvx_a_svy); d2J_dxdz_reading.block(0,valid_points_count,3,1) = tmp_vec3; // d2J_dxdz_reference double a_ctvx_a_stvy = cos(theta)*reference_direction(0) + sin(theta)*reference_direction(1); double m_stvx_a_ctvy = -sin(theta)*reference_direction(0) + cos(theta)*reference_direction(1); tmp1 = normal(1)*m_stvx_a_ctvy + normal(0)*a_ctvx_a_stvy; tmp_vec3(0) = 2.0 * normal(0) * tmp1; tmp_vec3(1) = 2.0 * normal(1) * tmp1; double tmp2 = normal(1)*m_svx_a_cvy + normal(0)*a_cvx_a_svy; tmp_vec3(2) = 2.0*reference_range * tmp2 * tmp1 + 2.0*tmp2 * (normal(0)*(reference_range*a_ctvx_a_stvy - reading_range*reading_direction(0) + x) + normal(1)*(reference_range*m_stvx_a_ctvy - reading_range*reading_direction(1) + y)); d2J_dxdz_reference.block(0,valid_points_count,3,1) = tmp_vec3; // valid counter valid_points_count++; } } // only cut out the valid d2J_dxdz Eigen::MatrixXd d2J_dxdz = Eigen::MatrixXd::Zero(3, 2 * valid_points_count); d2J_dxdz.block(0,0,3,valid_points_count) = d2J_dxdz_reading.block(0,0,3,valid_points_count); d2J_dxdz.block(0,valid_points_count,3,valid_points_count) = d2J_dxdz_reference.block(0,0,3,valid_points_count); Eigen::Matrix3d inv_J = d2J_dx2.inverse(); Eigen::Matrix3d cov = param_.sensor_sdv * param_.sensor_sdv * inv_J * (d2J_dxdz * d2J_dxdz.transpose()) * inv_J; return cov; }
// faster mode: if batch meory copy possible, call this to save time dealing with memory MatchResult ICPMatcher::matchPointClouds(const Eigen::MatrixXd &queryMat, const Eigen::MatrixXd &refMat, const gtsam::Pose2 &initpose) { MatchResult result; // pipe data in PMatcher::DataPoints::Labels label; label.push_back(PMatcher::DataPoints::Label("x",1)); label.push_back(PMatcher::DataPoints::Label("y",1)); label.push_back(PMatcher::DataPoints::Label("pad",1)); const PMatcher::DataPoints query(queryMat, label); const PMatcher::DataPoints ref(refMat, label); // perform icp and catch possible converge error PMatcher::TransformationParameters trans; try { trans = icp_(query, ref, initpose.matrix()); // check whether reach iteration if ((*icp_.transformationCheckers.begin())->getConditionVariables()[0] >= (*icp_.transformationCheckers.begin())->getLimits()[0]) { result.status = false; return result; } } catch (PMatcher::ConvergenceError error) { // catch error, cannot match result.status = false; return result; } // compute covariance, prepare all things needed Eigen::Matrix3d cov; // filtered point cloud PMatcher::DataPoints query_after_filter(query), ref_after_filter(ref); icp_.readingDataPointsFilters.apply(query_after_filter); icp_.referenceDataPointsFilters.apply(ref_after_filter); // apply PMatcher::DataPoints stepReading(query_after_filter); icp_.matcher->init(ref_after_filter); icp_.transformations.apply(stepReading, trans); // matches PMatcher::Matches matches = icp_.matcher->findClosests(stepReading); // outlier weight PMatcher::OutlierWeights outlier_weight = icp_.outlierFilters.compute(stepReading, ref_after_filter, matches); cov = this->estimateCovariance(query_after_filter, ref_after_filter, matches, outlier_weight, trans); // prepare the result strcuture result.status = true; result.delta_pose = Pose2(trans); result.inlier_ratio = icp_.errorMinimizer->getPointUsedRatio(); result.cov = cov; return result; }