Skip to content

Nacho/robust kernel colorized icp #2497

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
theNded merged 9 commits into from
Oct 20, 2020
Merged

Nacho/robust kernel colorized icp #2497

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
2e6ee5d
Add weighting function for the per-row version of ComputeJTJandJTr.
nachovizzo Oct 16, 2020
5cf368c
Add weight vector to match ComputeJTJandJr new API.
nachovizzo Oct 16, 2020
c25e8d4
Expose weight parameter for ColorMapOptimizationJacobian
nachovizzo Oct 16, 2020
04a0653
Finish ColorizeICP re-binding in python
nachovizzo Oct 16, 2020
6ab6709
Move private function to unnamed namespace
nachovizzo Oct 16, 2020
0c9bcfc
Update python examples
nachovizzo Oct 16, 2020
974ddcc
Fix generic->geomtric
nachovizzo Oct 16, 2020
e8923ca
Fix tests.
nachovizzo Oct 18, 2020
e02aacf
Fix unit tests
nachovizzo Oct 19, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 1 addition & 2 deletions cpp/open3d/pipelines/color_map/ColorMapOptimization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -171,9 +171,8 @@ static void OptimizeImageCoorRigid(

auto f_lambda = [&](int i, Eigen::Vector6d& J_r, double& r,
double& w) {
w = 1.0;
jac.ComputeJacobianAndResidualRigid(
i, J_r, r, mesh, proxy_intensity, images_gray[c],
i, J_r, r, w, mesh, proxy_intensity, images_gray[c],
images_dx[c], images_dy[c], intr, extrinsic,
visibility_image_to_vertex[c],
option.image_boundary_margin_);
Expand Down
2 changes: 2 additions & 0 deletions cpp/open3d/pipelines/color_map/ColorMapOptimizationJacobian.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ void ColorMapOptimizationJacobian::ComputeJacobianAndResidualRigid(
int row,
Eigen::Vector6d& J_r,
double& r,
double& w,
const geometry::TriangleMesh& mesh,
const std::vector<double>& proxy_intensity,
const std::shared_ptr<geometry::Image>& images_gray,
Expand Down Expand Up @@ -74,6 +75,7 @@ void ColorMapOptimizationJacobian::ComputeJacobianAndResidualRigid(
J_r(4) = v1;
J_r(5) = v2;
r = (gray - proxy_intensity[vid]);
w = 1.0;
}

void ColorMapOptimizationJacobian::ComputeJacobianAndResidualNonRigid(
Expand Down
1 change: 1 addition & 0 deletions cpp/open3d/pipelines/color_map/ColorMapOptimizationJacobian.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ class ColorMapOptimizationJacobian {
int row,
Eigen::Vector6d& J_r,
double& r,
double& w,
const geometry::TriangleMesh& mesh,
const std::vector<double>& proxy_intensity,
const std::shared_ptr<geometry::Image>& images_gray,
Expand Down
4 changes: 2 additions & 2 deletions cpp/open3d/pipelines/odometry/Odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -433,9 +433,9 @@ static std::tuple<bool, Eigen::Matrix4d> DoSingleIteration(
auto f_lambda =
[&](int i,
std::vector<Eigen::Vector6d, utility::Vector6d_allocator> &J_r,
std::vector<double> &r) {
std::vector<double> &w, std::vector<double> &r) {
jacobian_method.ComputeJacobianAndResidual(
i, J_r, r, source, target, source_xyz, target_dx,
i, J_r, r, w, source, target, source_xyz, target_dx,
target_dy, intrinsic, extrinsic_initial,
*correspondence);
};
Expand Down
7 changes: 7 additions & 0 deletions cpp/open3d/pipelines/odometry/RGBDOdometryJacobian.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ void RGBDOdometryJacobianFromColorTerm::ComputeJacobianAndResidual(
int row,
std::vector<Eigen::Vector6d, utility::Vector6d_allocator> &J_r,
std::vector<double> &r,
std::vector<double> &w,
const geometry::RGBDImage &source,
const geometry::RGBDImage &target,
const geometry::Image &source_xyz,
Expand Down Expand Up @@ -82,12 +83,15 @@ void RGBDOdometryJacobianFromColorTerm::ComputeJacobianAndResidual(
J_r[0](5) = c2;
r.resize(1);
r[0] = diff;
w.resize(1);
w[0] = 1.0;
}

void RGBDOdometryJacobianFromHybridTerm::ComputeJacobianAndResidual(
int row,
std::vector<Eigen::Vector6d, utility::Vector6d_allocator> &J_r,
std::vector<double> &r,
std::vector<double> &w,
const geometry::RGBDImage &source,
const geometry::RGBDImage &target,
const geometry::Image &source_xyz,
Expand Down Expand Up @@ -133,6 +137,7 @@ void RGBDOdometryJacobianFromHybridTerm::ComputeJacobianAndResidual(

J_r.resize(2);
r.resize(2);
w.resize(2);
J_r[0](0) = sqrt_lambda_img * (-p3d_trans(2) * c1 + p3d_trans(1) * c2);
J_r[0](1) = sqrt_lambda_img * (p3d_trans(2) * c0 - p3d_trans(0) * c2);
J_r[0](2) = sqrt_lambda_img * (-p3d_trans(1) * c0 + p3d_trans(0) * c1);
Expand All @@ -141,6 +146,7 @@ void RGBDOdometryJacobianFromHybridTerm::ComputeJacobianAndResidual(
J_r[0](5) = sqrt_lambda_img * (c2);
double r_photo = sqrt_lambda_img * diff_photo;
r[0] = r_photo;
w[0] = 1.0;

J_r[1](0) = sqrt_lamba_dep *
((-p3d_trans(2) * d1 + p3d_trans(1) * d2) - p3d_trans(1));
Expand All @@ -152,6 +158,7 @@ void RGBDOdometryJacobianFromHybridTerm::ComputeJacobianAndResidual(
J_r[1](5) = sqrt_lamba_dep * (d2 - 1.0f);
double r_geo = sqrt_lamba_dep * diff_geo;
r[1] = r_geo;
w[1] = 1.0;
}

} // namespace odometry
Expand Down
3 changes: 3 additions & 0 deletions cpp/open3d/pipelines/odometry/RGBDOdometryJacobian.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,7 @@ class RGBDOdometryJacobian {
int row,
std::vector<Eigen::Vector6d, utility::Vector6d_allocator> &J_r,
std::vector<double> &r,
std::vector<double> &w,
const geometry::RGBDImage &source,
const geometry::RGBDImage &target,
const geometry::Image &source_xyz,
Expand Down Expand Up @@ -99,6 +100,7 @@ class RGBDOdometryJacobianFromColorTerm : public RGBDOdometryJacobian {
int row,
std::vector<Eigen::Vector6d, utility::Vector6d_allocator> &J_r,
std::vector<double> &r,
std::vector<double> &w,
const geometry::RGBDImage &source,
const geometry::RGBDImage &target,
const geometry::Image &source_xyz,
Expand Down Expand Up @@ -129,6 +131,7 @@ class RGBDOdometryJacobianFromHybridTerm : public RGBDOdometryJacobian {
int row,
std::vector<Eigen::Vector6d, utility::Vector6d_allocator> &J_r,
std::vector<double> &r,
std::vector<double> &w,
const geometry::RGBDImage &source,
const geometry::RGBDImage &target,
const geometry::Image &source_xyz,
Expand Down
59 changes: 19 additions & 40 deletions cpp/open3d/pipelines/registration/ColoredICP.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,50 +32,22 @@
#include "open3d/geometry/KDTreeFlann.h"
#include "open3d/geometry/KDTreeSearchParam.h"
#include "open3d/geometry/PointCloud.h"
#include "open3d/pipelines/registration/RobustKernel.h"
#include "open3d/utility/Console.h"
#include "open3d/utility/Eigen.h"

namespace open3d {
namespace pipelines {
namespace registration {

namespace {

class PointCloudForColoredICP : public geometry::PointCloud {
public:
std::vector<Eigen::Vector3d> color_gradient_;
};

class TransformationEstimationForColoredICP : public TransformationEstimation {
public:
TransformationEstimationType GetTransformationEstimationType()
const override {
return type_;
};
TransformationEstimationForColoredICP(double lambda_geometric = 0.968)
: lambda_geometric_(lambda_geometric) {
if (lambda_geometric_ < 0 || lambda_geometric_ > 1.0)
lambda_geometric_ = 0.968;
}
~TransformationEstimationForColoredICP() override {}

public:
double ComputeRMSE(const geometry::PointCloud &source,
const geometry::PointCloud &target,
const CorrespondenceSet &corres) const override;
Eigen::Matrix4d ComputeTransformation(
const geometry::PointCloud &source,
const geometry::PointCloud &target,
const CorrespondenceSet &corres) const override;

public:
double lambda_geometric_;

private:
const TransformationEstimationType type_ =
TransformationEstimationType::ColoredICP;
};

static std::shared_ptr<PointCloudForColoredICP>
InitializePointCloudForColoredICP(
std::shared_ptr<PointCloudForColoredICP> InitializePointCloudForColoredICP(
const geometry::PointCloud &target,
const geometry::KDTreeSearchParamHybrid &search_param) {
utility::LogDebug("InitializePointCloudForColoredICP");
Expand Down Expand Up @@ -128,7 +100,7 @@ InitializePointCloudForColoredICP(
A(nn - 1, 2) = (nn - 1) * nt(2);
b(nn - 1, 0) = 0;
// solving linear equation
bool is_success;
bool is_success = false;
Eigen::MatrixXd x;
std::tie(is_success, x) = utility::SolveLinearSystemPSD(
A.transpose() * A, A.transpose() * b);
Expand All @@ -140,13 +112,16 @@ InitializePointCloudForColoredICP(
return output;
}

} // namespace

Eigen::Matrix4d TransformationEstimationForColoredICP::ComputeTransformation(
const geometry::PointCloud &source,
const geometry::PointCloud &target,
const CorrespondenceSet &corres) const {
if (corres.empty() || !target.HasNormals() || !target.HasColors() ||
!source.HasColors())
!source.HasColors()) {
return Eigen::Matrix4d::Identity();
}

double sqrt_lambda_geometric = sqrt(lambda_geometric_);
double lambda_photometric = 1.0 - lambda_geometric_;
Expand All @@ -157,7 +132,7 @@ Eigen::Matrix4d TransformationEstimationForColoredICP::ComputeTransformation(
auto compute_jacobian_and_residual =
[&](int i,
std::vector<Eigen::Vector6d, utility::Vector6d_allocator> &J_r,
std::vector<double> &r) {
std::vector<double> &r, std::vector<double> &w) {
size_t cs = corres[i][0];
size_t ct = corres[i][1];
const Eigen::Vector3d &vs = source.points_[cs];
Expand All @@ -166,10 +141,12 @@ Eigen::Matrix4d TransformationEstimationForColoredICP::ComputeTransformation(

J_r.resize(2);
r.resize(2);
w.resize(2);

J_r[0].block<3, 1>(0, 0) = sqrt_lambda_geometric * vs.cross(nt);
J_r[0].block<3, 1>(3, 0) = sqrt_lambda_geometric * nt;
r[0] = sqrt_lambda_geometric * (vs - vt).dot(nt);
w[0] = kernel_->Weight(r[0]);

// project vs into vt's tangential plane
Eigen::Vector3d vs_proj = vs - (vs - vt).dot(nt) * nt;
Expand All @@ -194,6 +171,7 @@ Eigen::Matrix4d TransformationEstimationForColoredICP::ComputeTransformation(
sqrt_lambda_photometric * vs.cross(ditM);
J_r[1].block<3, 1>(3, 0) = sqrt_lambda_photometric * ditM;
r[1] = sqrt_lambda_photometric * (is - is0_proj);
w[1] = kernel_->Weight(r[1]);
};

Eigen::Matrix6d JTJ;
Expand Down Expand Up @@ -249,13 +227,14 @@ RegistrationResult RegistrationColoredICP(
const geometry::PointCloud &target,
double max_distance,
const Eigen::Matrix4d &init /* = Eigen::Matrix4d::Identity()*/,
const ICPConvergenceCriteria &criteria /* = ICPConvergenceCriteria()*/,
double lambda_geometric /* = 0.968*/) {
const TransformationEstimationForColoredICP &estimation
/*TransformationEstimationForColoredICP()*/,
const ICPConvergenceCriteria
&criteria /* = ICPConvergenceCriteria()*/) {
auto target_c = InitializePointCloudForColoredICP(
target, geometry::KDTreeSearchParamHybrid(max_distance * 2.0, 30));
return RegistrationICP(
source, *target_c, max_distance, init,
TransformationEstimationForColoredICP(lambda_geometric), criteria);
return RegistrationICP(source, *target_c, max_distance, init, estimation,
criteria);
}

} // namespace registration
Expand Down
49 changes: 46 additions & 3 deletions cpp/open3d/pipelines/registration/ColoredICP.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,11 @@
#pragma once

#include <Eigen/Core>
#include <memory>

#include "open3d/pipelines/registration/Registration.h"
#include "open3d/pipelines/registration/RobustKernel.h"
#include "open3d/pipelines/registration/TransformationEstimation.h"

namespace open3d {

Expand All @@ -38,8 +41,45 @@ class PointCloud;

namespace pipelines {
namespace registration {

class RegistrationResult;

class TransformationEstimationForColoredICP : public TransformationEstimation {
public:
~TransformationEstimationForColoredICP() override{};

TransformationEstimationType GetTransformationEstimationType()
const override {
return type_;
};
explicit TransformationEstimationForColoredICP(
double lambda_geometric = 0.968,
std::shared_ptr<RobustKernel> kernel = std::make_shared<L2Loss>())
: lambda_geometric_(lambda_geometric), kernel_(std::move(kernel)) {
if (lambda_geometric_ < 0 || lambda_geometric_ > 1.0) {
lambda_geometric_ = 0.968;
}
}

public:
double ComputeRMSE(const geometry::PointCloud &source,
const geometry::PointCloud &target,
const CorrespondenceSet &corres) const override;
Eigen::Matrix4d ComputeTransformation(
const geometry::PointCloud &source,
const geometry::PointCloud &target,
const CorrespondenceSet &corres) const override;

public:
double lambda_geometric_ = 0.968;
/// shared_ptr to an Abstract RobustKernel that could mutate at runtime.
std::shared_ptr<RobustKernel> kernel_ = std::make_shared<L2Loss>();

private:
const TransformationEstimationType type_ =
TransformationEstimationType::ColoredICP;
};

/// \brief Function for Colored ICP registration.
///
/// This is implementation of following paper
Expand All @@ -52,14 +92,17 @@ class RegistrationResult;
/// \param init Initial transformation estimation.
/// Default value: array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.],
/// [0., 0., 0., 1.]]). \param criteria Convergence criteria. \param
/// lambda_geometric lambda_geometric value.
/// \param estimation TransformationEstimationForColoredICP method. Can only
/// change the lambda_geometric value and the robust kernel used in the
/// optimization
RegistrationResult RegistrationColoredICP(
const geometry::PointCloud &source,
const geometry::PointCloud &target,
double max_distance,
const Eigen::Matrix4d &init = Eigen::Matrix4d::Identity(),
const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria(),
double lambda_geometric = 0.968);
const TransformationEstimationForColoredICP &estimation =
TransformationEstimationForColoredICP(),
const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria());

} // namespace registration
} // namespace pipelines
Expand Down
9 changes: 6 additions & 3 deletions cpp/open3d/utility/Eigen.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -207,6 +207,7 @@ std::tuple<MatType, VecType, double> ComputeJTJandJTr(
std::function<
void(int,
std::vector<VecType, Eigen::aligned_allocator<VecType>> &,
std::vector<double> &,
std::vector<double> &)> f,
int iteration_num,
bool verbose /*=true*/) {
Expand All @@ -223,13 +224,14 @@ std::tuple<MatType, VecType, double> ComputeJTJandJTr(
JTJ_private.setZero();
JTr_private.setZero();
std::vector<double> r;
std::vector<double> w;
std::vector<VecType, Eigen::aligned_allocator<VecType>> J_r;
#pragma omp for nowait
for (int i = 0; i < iteration_num; i++) {
f(i, J_r, r);
f(i, J_r, r, w);
for (int j = 0; j < (int)r.size(); j++) {
JTJ_private.noalias() += J_r[j] * J_r[j].transpose();
JTr_private.noalias() += J_r[j] * r[j];
JTJ_private.noalias() += J_r[j] * w[j] * J_r[j].transpose();
JTr_private.noalias() += J_r[j] * w[j] * r[j];
r2_sum_private += r[j] * r[j];
}
}
Expand All @@ -255,6 +257,7 @@ template std::tuple<Eigen::Matrix6d, Eigen::Vector6d, double> ComputeJTJandJTr(
template std::tuple<Eigen::Matrix6d, Eigen::Vector6d, double> ComputeJTJandJTr(
std::function<void(int,
std::vector<Eigen::Vector6d, Vector6d_allocator> &,
std::vector<double> &,
std::vector<double> &)> f,
int iteration_num, bool verbose);
// clang-format on
Expand Down
1 change: 1 addition & 0 deletions cpp/open3d/utility/Eigen.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,7 @@ std::tuple<MatType, VecType, double> ComputeJTJandJTr(
std::function<
void(int,
std::vector<VecType, Eigen::aligned_allocator<VecType>> &,
std::vector<double> &,
std::vector<double> &)> f,
int iteration_num,
bool verbose = true);
Expand Down
1 change: 1 addition & 0 deletions cpp/pybind/pipelines/odometry/odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ class PyRGBDOdometryJacobian : public RGBDOdometryJacobianBase {
int row,
std::vector<Eigen::Vector6d, utility::Vector6d_allocator> &J_r,
std::vector<double> &r,
std::vector<double> &w,
const geometry::RGBDImage &source,
const geometry::RGBDImage &target,
const geometry::Image &source_xyz,
Expand Down
Loading