Colored point cloud registration & Multi-scale approach

[syncle]

I am trying to implant our ICCV code to Open3D.

The files I am editing are core/registration/TransformationEstimation.h/cpp and Registration.cpp. The question is the following virtual function:

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

for colored points registration, we need additional argument e.g. point-wise color gradient for computing Jacobian. Any good idea for this?

In addition, we know the multi-scale registration really helps registration fidelity. Our ICCV implementation code is based on this idea, but I would like to make a new wrapper class so all the ICP methods in Open3D can get benefits. How do you think?

[qianyizh]

Let me answer the easier question first:

The multi-scale thing can be in the python script.

We can have an additional layer in C++. E.g., a function called ICPRegistrationMultiScale, but we do not encourage user to use it.

The reason is that the parameters might need to be tune. So in this case, we leave the multi-scale function in python is a good choice.

[qianyizh]

Now the harder question:

Define a class called TransformationEstimationForColoredPointCloud.
The additional data structure like point cloud gradients are the class variables of this class.
Then we do a "lazy initialization" for these additional data. Things like this:

class TransformationEstimationForColoredPointCloud: public TransformationEstimation
{
public:
	TransformationEstimationForColoredPointCloud() {}
	~TransformationEstimationForColoredPointCloud() override {}

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

private:
	void InitializeGradient(const PointCloud &target);

private:
	std::vector<Eigen::Vector3d> target_color_gradient_;
};

Eigen::Matrix4d TransformationEstimationForColoredPointCloud::ComputeTransformation(
		const PointCloud &source, const PointCloud &target,
		const CorrespondenceSet &corres) const
{
	if (corres.empty() || target.HasNormals() == false || target.HasColor() == false || source.HasColor == false)
		return Eigen::Matrix4d::Identity();
	if (target_color_gradient_.size() != target.points_.size())
		InitializeGradient();

	......
	......
}

[qianyizh]

Yeah, this looks great!

And I think it is just plug and play!
We can still use the RegistrationICP function, but use the new TransformationEstimationForColoredPointCloud class. It should be fairly easy!

[qianyizh]

wait, there is one problem, that we cannot make estimate const &, like this:

RegistrationResult RegistrationICP(const PointCloud &source,
		const PointCloud &target, double max_correspondence_distance,
		const Eigen::Matrix4d &init/* = Eigen::Matrix4d::Identity()*/,
		const TransformationEstimation &estimation
		/* = TransformationEstimationPointToPoint(false)*/,
		const ICPConvergenceCriteria &criteria/* = ICPConvergenceCriteria()*/)

Let me think.

[qianyizh]

The easiest way is to follow my suggestions, with these additional changes:

1. Remove const from TransformationEstimation::ComputeRMSE() and TransformationEstimation::ComputeTransformation().

2. Change const TransformationEstimation &estimation in RegistrationICP and such definition to TransformationEstimation estimation.

I think it is viable and legitimate. After all, the purpose of using a class for TransformationEstimation is to store some intermediate results that can be useful (e.g., the color gradient).

What do you think?

[syncle]

Yes. I agree that this is appropriate way to extend TransformationEstimation. We don't have to stick to define estimation as const. Let me work on that based on this suggestion.

[syncle]

Let's make it clear,

So we want to change

class TransformationEstimation
{
public:
	TransformationEstimation() {}
	virtual ~TransformationEstimation() {}

public:
	virtual double ComputeRMSE(const PointCloud &source,
			const PointCloud &target,
			const CorrespondenceSet &corres) const = 0;
	virtual Eigen::Matrix4d ComputeTransformation(const PointCloud &source,
			const PointCloud &target,
			const CorrespondenceSet &corres) const = 0;
};

as to be

class TransformationEstimation
{
public:
	TransformationEstimation() {}
	virtual ~TransformationEstimation() {}

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

right? I had removed const in function definition. This change will affects TransformationEstimationPointToPoint, TransformationEstimationPointToPlane, and Python binding and any modules using TransformationEstimation.

[qianyizh]

Yes, I think so.

[syncle]

I just noticed that OS X prohibit referencing of non-constant type.

/// Functions for ICP registration
RegistrationResult RegistrationICP(const PointCloud &source,
		const PointCloud &target, double max_correspondence_distance,
		const Eigen::Matrix4d &init = Eigen::Matrix4d::Identity(),
		TransformationEstimation &estimation =
		TransformationEstimationPointToPoint(false),
		const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria());

This will makes following error:

/Users/jaesikpa/Research/open3d_dev/Open3D/src/Core/Registration/Registration.h:103:29: error: 
      non-const lvalue reference to type 'three::TransformationEstimation'
      cannot bind to a temporary of type
      'three::TransformationEstimationPointToPoint'
                TransformationEstimation &estimation =
                                          ^

If I remove & operator in TransformationEstimation &estimation =, it produces following error:

/Users/jaesikpa/Research/open3d_dev/Open3D/src/Core/Registration/Registration.h:103:28: error: 
      parameter type 'three::TransformationEstimation' is an abstract class
                TransformationEstimation estimation =

This error is not showed up in Visual Studio. Am I doing some stupid mistake?

[qianyizh]

Yes, you cannot use reference. Because the default initializer produce a const TransformationEstimationPointToPoint(false).

You cannot remove & either, because first you need to overload operator =, second the bund functions then will be of TransformationEstimation not the subclasses.

I think the easiest fix is to remove the default initializer. So try this:

/// Functions for ICP registration
RegistrationResult RegistrationICP(const PointCloud &source,
		const PointCloud &target, double max_correspondence_distance,
		TransformationEstimation &estimation,
		const Eigen::Matrix4d &init = Eigen::Matrix4d::Identity(),
		const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria());

The downside though, is that it is really ugly. And we cannot use function calls like:

RegistrationICP(source, target, TransformationEstimationPointToPoint(false), 0.01);

Do you have any better ideas?

[syncle]

Well I guess we don't have enough choice. I don't have better idea for now.
Removing default initializer.

[qianyizh]

Let me think. I actually think we can restructure the entire thing.
Maybe use lambda functions to replace the entire structure of TransformationEstimation.

But let me think a bit.

[syncle]

Based on the discussion today, I understood that following code structure

in PointCloudForColoredICP.h

namespace three {

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

void InitializeGradient(PointCloudForColoredICP target);

Eigen::Matrix4d ComputeTransformationForColoredICP(args);

}

in TransformationEstimation.h

class TransformationEstimationColoredICP : public TransformationEstimation
{
:
}

in TransformationEstimation.cpp

Eigen::Matrix4d TransformationEstimationColoredICP::ComputeTransformation(
		const PointCloud &source, const PointCloud &target,
		const CorrespondenceSet &corres) const
{
   PointCloudForColoredICP source_c = source;
   PointCloudForColoredICP target_c = target;
   InitializeGradient(target);
   Eigen::Matrix4d transform = ComputeTransformationForColoredICP(args)
}

Any mistakes I made?

[syncle]

Regarding the line PointCloudForColoredICP source_c = source;,
there is a dispute about assigning derived class from base class.

People regard it as not good idea because the variables in derived class would be not initialized.

Check out this link

[qianyizh]

Everything should be in two files:

ColoredICP.h
ColoredICP.cpp

First, put proper citation to our paper in the header file, right before the entrance function.

Here is the .cpp

namespace {

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

class TransformationEstimationForColoredICP : public TransformationEstimation {
       Eigen::Matrix4 ComputeTransformation(
		const PointCloud &source, const PointCloud &target,
		const CorrespondenceSet &corres) const {
                const auto &target_c = (const PointCloudForColoredICP &)target;
                ...
                ...
        }
};

PointCloudForColoredICP InitializePointCloudForColoredICP(const PointCloud &cloud) {
    // create a new point cloud, and do the initialization here.
}

}

RegistrationResult RegistrationColoredICP(const PointCloud &source,
    const PointCloud &target, double max_distance, ...)   // no TransformationEstimation in the parameters
{
    target_c = InitializePointCloudForColoredICP(target);
    RegistrationICP(source, target_c, max_distance, TransformationEstimationForColoredICP(), ...)
}

[syncle]

Seems good, but I am not clear following line.

RegistrationICP(source, target_c, max_distance, TransformationEstimationForColoredICP(), ...)

RegistrationICP only can take PointCloud. Do you mean overloading RegistrationICP?