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