void ICPSlowdometry::initICPModel(unsigned short * depth,
const float depthCutoff,
const Eigen::Matrix4f & modelPose)
{
depth_tmp[0].upload(depth, sizeof(unsigned short) * width, height, width);
for(int i = 1; i < NUM_PYRS; ++i)
{
pyrDown(depth_tmp[i - 1], depth_tmp[i]);
}
for(int i = 0; i < NUM_PYRS; ++i)
{
createVMap(intr(i), depth_tmp[i], vmaps_g_prev_[i], depthCutoff);
createNMap(vmaps_g_prev_[i], nmaps_g_prev_[i]);
}
cudaDeviceSynchronize();
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcam = modelPose.topLeftCorner(3, 3);
Eigen::Vector3f tcam = modelPose.topRightCorner(3, 1);
Mat33 & device_Rcam = device_cast<Mat33>(Rcam);
float3& device_tcam = device_cast<float3>(tcam);
for(int i = 0; i < NUM_PYRS; ++i)
{
tranformMaps(vmaps_g_prev_[i], nmaps_g_prev_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]);
}
cudaDeviceSynchronize();
}
void ICPOdometry::initICPModel(const DeviceArray2D<unsigned short>& depth,
const float depthCutoff,
const Eigen::Matrix4f & modelPose)
{
depth_tmp[0] = depth;
for(int i = 1; i < NUM_PYRS; ++i)
{
pyrDown(depth_tmp[i - 1], depth_tmp[i]);
}
for(int i = 0; i < NUM_PYRS; ++i)
{
createVMap(intr(i), depth_tmp[i], vmaps_g_prev_[i], depthCutoff);
createNMap(vmaps_g_prev_[i], nmaps_g_prev_[i]);
}
cudaDeviceSynchronize();
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcam = modelPose.topLeftCorner(3, 3);
Eigen::Vector3f tcam = modelPose.topRightCorner(3, 1);
Mat33 & device_Rcam = device_cast<Mat33>(Rcam);
float3& device_tcam = device_cast<float3>(tcam);
if (modelPose != Eigen::Matrix4f::Identity())
for(int i = 0; i < NUM_PYRS; ++i)
tranformMaps(vmaps_g_prev_[i], nmaps_g_prev_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]);
cudaDeviceSynchronize();
}
void DiriniVisualizer::addCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_, Eigen::Matrix3f R, Eigen::Vector3f t)
{
Eigen::Matrix4f H = Eigen::Matrix4f::Identity();
H.topLeftCorner(3,3) = R;
H.topRightCorner(3,1) = t;
addCloud(cloud_, H);
}
void DiriniVisualizer::addCloud(boost::shared_ptr<std::vector<Eigen::Vector3i> > points, Eigen::Matrix3f R, Eigen::Vector3f t)
{
Eigen::Matrix4f H = Eigen::Matrix4f::Identity();
H.topLeftCorner(3,3) = R;
H.topRightCorner(3,1) = t;
addCloud(points, H);
}
void RGBDOdometry::initICPModel(GPUTexture * predictedVertices,
GPUTexture * predictedNormals,
const float depthCutoff,
const Eigen::Matrix4f & modelPose)
{
cudaArray * textPtr;
cudaGraphicsMapResources(1, &predictedVertices->cudaRes);
cudaGraphicsSubResourceGetMappedArray(&textPtr, predictedVertices->cudaRes, 0, 0);
cudaMemcpyFromArray(vmaps_tmp.ptr(), textPtr, 0, 0, vmaps_tmp.sizeBytes(), cudaMemcpyDeviceToDevice);
cudaGraphicsUnmapResources(1, &predictedVertices->cudaRes);
cudaGraphicsMapResources(1, &predictedNormals->cudaRes);
cudaGraphicsSubResourceGetMappedArray(&textPtr, predictedNormals->cudaRes, 0, 0);
cudaMemcpyFromArray(nmaps_tmp.ptr(), textPtr, 0, 0, nmaps_tmp.sizeBytes(), cudaMemcpyDeviceToDevice);
cudaGraphicsUnmapResources(1, &predictedNormals->cudaRes);
copyMaps(vmaps_tmp, nmaps_tmp, vmaps_g_prev_[0], nmaps_g_prev_[0]);
for(int i = 1; i < NUM_PYRS; ++i)
{
resizeVMap(vmaps_g_prev_[i - 1], vmaps_g_prev_[i]);
resizeNMap(nmaps_g_prev_[i - 1], nmaps_g_prev_[i]);
}
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcam = modelPose.topLeftCorner(3, 3);
Eigen::Vector3f tcam = modelPose.topRightCorner(3, 1);
mat33 device_Rcam = Rcam;
float3 device_tcam = *reinterpret_cast<float3*>(tcam.data());
for(int i = 0; i < NUM_PYRS; ++i)
{
tranformMaps(vmaps_g_prev_[i], nmaps_g_prev_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]);
}
cudaDeviceSynchronize();
}
bool Feature::getTransformationByRANSAC(Eigen::Matrix4f &result_transform,
Eigen::Matrix<double, 6, 6> &result_information,
int &point_count, int &point_corr_count,
float &rmse, vector<cv::DMatch> *matches, // output vars. if matches == nullptr, do not return inlier match
const Feature* earlier, const Feature* now,
PointCloudCuda *pcc,
const vector<cv::DMatch> &initial_matches)
{
if (matches != nullptr) matches->clear();
if (initial_matches.size() < (unsigned int) Config::instance()->get<int>("min_matches"))
{
return false;
}
unsigned int min_inlier_threshold = (unsigned int)(initial_matches.size() * Config::instance()->get<float>("min_inliers_percent"));
std::vector<cv::DMatch> inlier; //holds those feature correspondences that support the transformation
float inlier_error; //all squared errors
srand((long)std::clock());
// a point is an inlier if it's no more than max_dist_m m from its partner apart
const float max_dist_m = Config::instance()->get<float>("max_dist_for_inliers");
const float squared_max_dist_m = max_dist_m * max_dist_m;
// best values of all iterations (including invalids)
float best_error = 1e6, best_error_invalid = 1e6;
unsigned int best_inlier_invalid = 0, best_inlier_cnt = 0, valid_iterations = 0;
Eigen::Matrix4f transformation;
const unsigned int sample_size = 3;// chose this many randomly from the correspondences:
unsigned int ransac_iterations = Config::instance()->get<int>("ransac_max_iteration");
int threads = Config::instance()->get<int>("icpcuda_threads");
int blocks = Config::instance()->get<int>("icpcuda_blocks");
float corr_percent = Config::instance()->get<float>("coresp_percent");
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
int pc = 1.0, pcorrc = 0.0;
bool *used = new bool[initial_matches.size()];
memset(used, 0, initial_matches.size() * sizeof(bool));
vector<int> sample_matches_indices(3);
vector<cv::DMatch> sample_matches_vector(3);
for (unsigned int n_iter = 0; n_iter < ransac_iterations; n_iter++)
{
for (int i = 0; i < sample_matches_indices.size(); i++)
{
used[sample_matches_indices[i]] = false;
}
sample_matches_indices.clear();
sample_matches_vector.clear();
while (sample_matches_indices.size() < sample_size)
{
int id = rand() % initial_matches.size();
if (!used[id])
{
used[id] = true;
sample_matches_indices.push_back(id);
sample_matches_vector.push_back(initial_matches.at(id));
}
}
bool valid; // valid is false iff the sampled points clearly aren't inliers themself
transformation = Feature::getTransformFromMatches(valid,
earlier->feature_pts_3d,
now->feature_pts_3d,
sample_matches_vector, max_dist_m);
if (!valid) continue; // valid is false iff the sampled points aren't inliers themself
if (transformation != transformation) continue; //Contains NaN
//test whether samples are inliers (more strict than before)
Feature::computeInliersAndError(inlier, inlier_error, nullptr,
sample_matches_vector, transformation,
earlier->feature_pts_3d, now->feature_pts_3d,
squared_max_dist_m);
if (inlier_error > 1000) continue; //most possibly a false match in the samples
Feature::computeInliersAndError(inlier, inlier_error, nullptr,
initial_matches, transformation,
earlier->feature_pts_3d, now->feature_pts_3d,
squared_max_dist_m);
if (inlier.size() < min_inlier_threshold || inlier_error > max_dist_m)
{
continue;
}
valid_iterations++;
//Performance hacks:
///Iterations with more than half of the initial_matches inlying, count twice
if (inlier.size() > initial_matches.size() * 0.5) n_iter++;
///Iterations with more than 80% of the initial_matches inlying, count threefold
if (inlier.size() > initial_matches.size() * 0.8) n_iter++;
if (inlier_error < best_error)
{ //copy this to the result
if (pcc != nullptr)
{
Eigen::Vector3f t = transformation.topRightCorner(3, 1);
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rot = transformation.topLeftCorner(3, 3);
pcc->getCoresp(t, rot, information, pc, pcorrc, threads, blocks);
}
result_transform = transformation;
result_information = information;
point_count = pc;
point_corr_count = pcorrc;
if (matches != nullptr) *matches = inlier;
best_inlier_cnt = inlier.size();
rmse = inlier_error;
best_error = inlier_error;
}
float new_inlier_error;
transformation = Feature::getTransformFromMatches(valid,
earlier->feature_pts_3d, now->feature_pts_3d, inlier); // compute new trafo from all inliers:
if (transformation != transformation) continue; //Contains NaN
Feature::computeInliersAndError(inlier, new_inlier_error, nullptr,
initial_matches, transformation,
earlier->feature_pts_3d, now->feature_pts_3d,
squared_max_dist_m);
if(inlier.size() < min_inlier_threshold || new_inlier_error > max_dist_m)
{
continue;
}
if (new_inlier_error < best_error)
{
if (pcc != nullptr)
{
Eigen::Vector3f t = transformation.topRightCorner(3, 1);
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rot = transformation.topLeftCorner(3, 3);
pcc->getCoresp(t, rot, information, pc, pcorrc, threads, blocks);
}
result_transform = transformation;
if (pcc != nullptr)
result_information = information;
point_count = pc;
point_corr_count = pcorrc;
if (matches != nullptr) *matches = inlier;
best_inlier_cnt = inlier.size();
rmse = new_inlier_error;
best_error = new_inlier_error;
}
} //iterations
if (pcc == nullptr)
result_information = Eigen::Matrix<double, 6, 6>::Identity();
return best_inlier_cnt >= min_inlier_threshold;
}
void DiriniVisualizer::addPoint(Eigen::Vector3f point, Eigen::Matrix4f transform_)
{
points.push_back(transform_.topLeftCorner(3,3)*point+transform_.topRightCorner(3,1));
}
