template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
typename std::vector<Scalar>::const_iterator& weights_it,
Matrix4 &transformation_matrix) const
{
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
Matrix6d ATA;
Vector6d ATb;
ATA.setZero ();
ATb.setZero ();
while (source_it.isValid () && target_it.isValid ())
{
if (!pcl_isfinite (source_it->x) ||
!pcl_isfinite (source_it->y) ||
!pcl_isfinite (source_it->z) ||
!pcl_isfinite (target_it->x) ||
!pcl_isfinite (target_it->y) ||
!pcl_isfinite (target_it->z) ||
!pcl_isfinite (target_it->normal_x) ||
!pcl_isfinite (target_it->normal_y) ||
!pcl_isfinite (target_it->normal_z))
{
++ source_it;
++ target_it;
++ weights_it;
continue;
}
const float & sx = source_it->x;
const float & sy = source_it->y;
const float & sz = source_it->z;
const float & dx = target_it->x;
const float & dy = target_it->y;
const float & dz = target_it->z;
const float & nx = target_it->normal[0] * (*weights_it);
const float & ny = target_it->normal[1] * (*weights_it);
const float & nz = target_it->normal[2] * (*weights_it);
double a = nz*sy - ny*sz;
double b = nx*sz - nz*sx;
double c = ny*sx - nx*sy;
// 0 1 2 3 4 5
// 6 7 8 9 10 11
// 12 13 14 15 16 17
// 18 19 20 21 22 23
// 24 25 26 27 28 29
// 30 31 32 33 34 35
ATA.coeffRef (0) += a * a;
ATA.coeffRef (1) += a * b;
ATA.coeffRef (2) += a * c;
ATA.coeffRef (3) += a * nx;
ATA.coeffRef (4) += a * ny;
ATA.coeffRef (5) += a * nz;
ATA.coeffRef (7) += b * b;
ATA.coeffRef (8) += b * c;
ATA.coeffRef (9) += b * nx;
ATA.coeffRef (10) += b * ny;
ATA.coeffRef (11) += b * nz;
ATA.coeffRef (14) += c * c;
ATA.coeffRef (15) += c * nx;
ATA.coeffRef (16) += c * ny;
ATA.coeffRef (17) += c * nz;
ATA.coeffRef (21) += nx * nx;
ATA.coeffRef (22) += nx * ny;
ATA.coeffRef (23) += nx * nz;
ATA.coeffRef (28) += ny * ny;
ATA.coeffRef (29) += ny * nz;
ATA.coeffRef (35) += nz * nz;
double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
ATb.coeffRef (0) += a * d;
ATb.coeffRef (1) += b * d;
ATb.coeffRef (2) += c * d;
ATb.coeffRef (3) += nx * d;
ATb.coeffRef (4) += ny * d;
ATb.coeffRef (5) += nz * d;
++ source_it;
++ target_it;
++ weights_it;
}
ATA.coeffRef (6) = ATA.coeff (1);
ATA.coeffRef (12) = ATA.coeff (2);
ATA.coeffRef (13) = ATA.coeff (8);
ATA.coeffRef (18) = ATA.coeff (3);
ATA.coeffRef (19) = ATA.coeff (9);
ATA.coeffRef (20) = ATA.coeff (15);
ATA.coeffRef (24) = ATA.coeff (4);
ATA.coeffRef (25) = ATA.coeff (10);
ATA.coeffRef (26) = ATA.coeff (16);
ATA.coeffRef (27) = ATA.coeff (22);
ATA.coeffRef (30) = ATA.coeff (5);
ATA.coeffRef (31) = ATA.coeff (11);
ATA.coeffRef (32) = ATA.coeff (17);
ATA.coeffRef (33) = ATA.coeff (23);
ATA.coeffRef (34) = ATA.coeff (29);
// Solve A*x = b
Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
// Construct the transformation matrix from x
constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}
void DirectPoseEstimationSingleLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> depth_ref,
Sophus::SE3 &T21
) {
// parameters
int half_patch_size = 4;
int iterations = 100;
double cost = 0, lastCost = 0;
int nGood = 0; // good projections
VecVector2d goodProjection;
for (int iter = 0; iter < iterations; iter++) {
nGood = 0;
goodProjection.clear();
// Define Hessian and bias
Matrix6d H = Matrix6d::Zero(); // 6x6 Hessian
Vector6d b = Vector6d::Zero(); // 6x1 bias
for (size_t i = 0; i < px_ref.size(); i++) {
// compute the projection in the second image
// TODO START YOUR CODE HERE
float u =0, v = 0;
Eigen::Vector2d p1 = px_ref[i];
Eigen::Vector3d pw((p1(0) - cx) / fx * depth_ref[i], (p1(1) - cy) / fy * depth_ref[i], depth_ref[i]);
Eigen::Vector3d pc = T21 * pw;
Eigen::Vector2d p2(fx * pc(0) / pc(2) + cx, fy * pc(1) / pc(2) + cy);
u = p2(0);
v = p2(1);
if(u <= half_patch_size + 1 || u >=img2.cols - half_patch_size - 1 || v <= half_patch_size - 1 || v >= img2.rows - half_patch_size - 1)
{
continue;
}
nGood++;
goodProjection.push_back(Eigen::Vector2d(u, v));
// and compute error and jacobian
for (int x = -half_patch_size; x < half_patch_size; x++)
for (int y = -half_patch_size; y < half_patch_size; y++) {
double error = GetPixelValue(img1, p1(0)+x, p1(1)+y) - GetPixelValue(img2, p2(0)+x, p2(1)+y);
double Z = depth_ref[i];
double X = (p1(0) + x - cx) / fx * Z;
double Y = (p1(1) + y - cy) / fy * Z;
Matrix26d J_pixel_xi; // pixel to \xi in Lie algebra
J_pixel_xi(0, 0) = fx / Z;
J_pixel_xi(0, 1) = 0;
J_pixel_xi(0, 2) = -fx * X / Z / Z;
J_pixel_xi(0, 3) = -fx * X * Y / Z / Z;
J_pixel_xi(0, 4) = fx + fx * X * X / Z / Z;
J_pixel_xi(0, 5) = -fx * Y / Z;
J_pixel_xi(1, 0) = 0;
J_pixel_xi(1, 1) = fy / Z;
J_pixel_xi(1, 2) = -fy * Y / Z / Z;
J_pixel_xi(1, 3) = -fy - fy * Y * Y / Z / Z;
J_pixel_xi(1, 4) = fy * X * Y / Z / Z;
J_pixel_xi(1, 5) = fy * X / Z;
Eigen::Vector2d J_img_pixel; // image gradients
J_img_pixel[0] = (GetPixelValue(img2, p2(0)+x+1, p2(1)+y) - GetPixelValue(img2,p2(0)+x-1,p2(1)+y))/2;
J_img_pixel[1] = (GetPixelValue(img2, p2(0)+x, p2(1)+y+1) - GetPixelValue(img2,p2(0)+x,p2(1)+y-1))/2;
// total jacobian
Vector6d J= -J_img_pixel.transpose() * J_pixel_xi;
H += J * J.transpose();
b += -error * J;
cost += error * error;
}
// END YOUR CODE HERE
}
// solve update and put it into estimation
// TODO START YOUR CODE HERE
Vector6d update = H.inverse() * b;
T21 = Sophus::SE3::exp(update) * T21;
// END YOUR CODE HERE
cost /= nGood;
if (isnan(update[0])) {
// sometimes occurred when we have a black or white patch and H is irreversible
cout << "update is nan" << endl;
break;
}
if (iter > 0 && cost > lastCost) {
cout << "cost increased: " << cost << ", " << lastCost << endl;
break;
}
lastCost = cost;
cout << "cost = " << cost << ", good = " << nGood << endl;
}
cout << "good projection: " << nGood << endl;
cout << "T21 = \n" << T21.matrix() << endl;
// in order to help you debug, we plot the projected pixels here
cv::Mat img1_show, img2_show;
cv::cvtColor(img1, img1_show, CV_GRAY2BGR);
cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
for (auto &px: px_ref) {
cv::rectangle(img1_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
cv::Scalar(0, 250, 0));
}
for (auto &px: goodProjection) {
cv::rectangle(img2_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
cv::Scalar(0, 250, 0));
}
cv::imshow("reference", img1_show);
cv::imshow("current", img2_show);
cv::waitKey();
}
