void render3d(std::string file_name, size_t width, size_t height) {
Renderer3d renderer = Renderer3d(file_name);
double near = 0.1, far = 1000;
double focal_length_x = 525, focal_length_y = 525;
renderer.set_parameters(width, height, focal_length_x, focal_length_y, near, far);
RendererIterator renderer_iterator = RendererIterator(&renderer, 150);
cv::Rect rect;
cv::Mat image, depth, mask;
for (size_t i = 0; !renderer_iterator.isDone(); ++i, ++renderer_iterator) {
try {
renderer_iterator.render(image, depth, mask, rect);
cv::imwrite(boost::str(boost::format("depth_%05d.png") % (i)), depth);
cv::imwrite(boost::str(boost::format("image_%05d.png") % (i)), image);
cv::imwrite(boost::str(boost::format("mask_%05d.png") % (i)), mask);
} catch (...) {
}
}
}
void render_and_perform_icp(std::string file_name, std::string points_3d_ref_filename,
std::string points_3d_model_filename, size_t width, size_t height) {
Renderer3d renderer = Renderer3d(file_name);
double near = 0.3, far = 3500;
double focal_length_x = 530, focal_length_y = 530;
renderer.set_parameters(width, height, focal_length_x, focal_length_y, near, far);
RendererIterator renderer_iterator = RendererIterator(&renderer, 1);
Matx33f R_object_ref(-0.7366852281578607, -0.4999999600934258, -0.4552965127480917,
0.4253253695228831, -0.8660254268245088, 0.2628655362227054,
-0.5257311144056662, 0, 0.8506508069388852);
Vec3d T_object_ref(-0.36801174, 0.0, 0.5954555);
Matx33f R_object_model(-0.8506508159122358, 0, -0.5257310998864799,
-0, -1, 0,
-0.5257310998864799, 0, 0.8506508159122358);
Vec3d T_object_model(-0.368012, 0, 0.595456);
// extract the UP vector from R
cv::Vec3d UP_ref(-(R_object_ref.inv()(0,1)),
-(R_object_ref.inv()(1,1)),
-(R_object_ref.inv()(2,1)));
cv::Vec3d UP_model(-(R_object_model.inv()(0,1)),
-(R_object_model.inv()(1,1)),
-(R_object_model.inv()(2,1)));
cv::Rect rect;
cv::Mat image, depth, mask;
// render reference and model, just for visualization
try {
renderer_iterator.render(image, depth, mask, rect, T_object_ref, UP_ref);
imwrite("rendered_reference.png", image);
renderer_iterator.render(image, depth, mask, rect, T_object_model, UP_model);
imwrite("rendered_model.png", image);
} catch (...) {
std::cout << "RENDER EXCEPTION" << endl;
}
// retrieve the mats of 3d points
Mat_<cv::Vec3f> points_3d_ref = read_mat(points_3d_ref_filename);
Mat_<cv::Vec3f> points_3d_model = read_mat(points_3d_model_filename);
// show them --warning-- QT could show them a bit deformed, they are fine if saved with imwrite
visualize_3d_points(points_3d_ref, "reference 3d points");
visualize_3d_points(points_3d_model, "model 3d points");
// NOW PEFORM ICP
//initialize the translation based on reference data
cv::Vec3f T_icp = points_3d_ref(points_3d_ref.rows/ 2.0f, points_3d_ref.cols / 2.0f);
//add the object's depth
T_icp(2) += 0.065;
if (!cv::checkRange(T_icp))
CV_Assert(false);
//initialize the rotation based on model data
if (!cv::checkRange(R_object_model))
CV_Assert(false);
cv::Matx33f R_icp(R_object_model);
//get the point clouds (for both reference and model)
std::vector<cv::Vec3f> pts_real_model_temp;
std::vector<cv::Vec3f> pts_real_ref_temp;
float px_ratio_missing = matToVec(points_3d_model, points_3d_ref, pts_real_model_temp, pts_real_ref_temp);
if (px_ratio_missing > 0.25f)
CV_Assert(false);
//perform the first approximate ICP
float px_ratio_match_inliers = 0.0f;
float icp_dist = icpCloudToCloud(pts_real_ref_temp, pts_real_model_temp, R_icp, T_icp, px_ratio_match_inliers, 1);
cout <<"Distance after first ICP: "<<icp_dist<<endl;
//perform a finer ICP
icp_dist = icpCloudToCloud(pts_real_ref_temp, pts_real_model_temp, R_icp, T_icp, px_ratio_match_inliers, 2);
cout <<"Distance after second ICP: "<<icp_dist<<endl;
//perform the final precise icp
float icp_px_match = 0.0f;
icp_dist = icpCloudToCloud(pts_real_ref_temp, pts_real_model_temp, R_icp, T_icp, icp_px_match, 0);
// extract the UP vector from updated R
cv::Vec3d UP_icp(-(R_icp.inv()(0,1)),
-(R_icp.inv()(1,1)),
-(R_icp.inv()(2,1)));
// render result
try {
renderer_iterator.render(image, depth, mask, rect, T_icp, UP_icp);
imwrite("rendered_icp_result.png", image);
} catch (...) {
std::cout << "RENDER EXCEPTION" << endl;
}
cout << "------ICP output--------" << endl << endl;
cout << "R: "<< R_icp << endl <<endl;
cout << "T: "<< T_icp << endl <<endl;
cout <<"Final distance after last ICP: "<<icp_dist<<endl;
cout <<"icp_px_match: "<<icp_px_match<<endl;
waitKey(0);
}
int process(const tendrils& inputs, const tendrils& outputs) {
// Get the document for the object_id_ from the DB
object_recognition_core::db::ObjectDbPtr db =
object_recognition_core::db::ObjectDbParameters(*json_db_).generateDb();
object_recognition_core::db::Documents documents =
object_recognition_core::db::ModelDocuments(db,
std::vector<object_recognition_core::db::ObjectId>(1, *object_id_),
"mesh");
if (documents.empty()) {
std::cerr << "Skipping object id \"" << *object_id_ << "\" : no mesh in the DB" << std::endl;
return ecto::OK;
}
// Get the list of _attachments and figure out the original one
object_recognition_core::db::Document document = documents[0];
std::vector<std::string> attachments_names = document.attachment_names();
std::string mesh_path;
std::vector<std::string> possible_names(2);
possible_names[0] = "original";
possible_names[1] = "mesh";
for (size_t i = 0; i < possible_names.size() && mesh_path.empty(); ++i) {
BOOST_FOREACH(const std::string& attachment_name, attachments_names) {
if (attachment_name.find(possible_names[i]) != 0)
continue;
// Create a temporary file
char mesh_path_tmp[L_tmpnam];
tmpnam(mesh_path_tmp);
mesh_path = std::string(mesh_path_tmp) + attachment_name.substr(possible_names[i].size());
// Load the mesh and save it to the temporary file
std::ofstream mesh_file;
mesh_file.open(mesh_path.c_str());
document.get_attachment_stream(attachment_name, mesh_file);
mesh_file.close();
}
}
cv::Ptr<cv::linemod::Detector> detector_ptr = cv::linemod::getDefaultLINEMOD();
*detector_ = *detector_ptr;
// Define the display
//assign the parameters of the renderer
*renderer_n_points_ = *param_n_points_;
*renderer_angle_step_ = *param_angle_step_;
*renderer_radius_min_ = *param_radius_min_;
*renderer_radius_max_ = *param_radius_max_;
*renderer_radius_step_ = *param_radius_step_;
*renderer_width_ = *param_width_;
*renderer_height_ = *param_height_;
*renderer_near_ = *param_near_;
*renderer_far_ = *param_far_;
*renderer_focal_length_x_ = *param_focal_length_x_;
*renderer_focal_length_y_ = *param_focal_length_y_;
// the model name can be specified on the command line.
Renderer3d renderer = Renderer3d(mesh_path);
renderer.set_parameters(*renderer_width_, *renderer_height_, *renderer_focal_length_x_,
*renderer_focal_length_y_, *renderer_near_, *renderer_far_);
std::remove(mesh_path.c_str());
RendererIterator renderer_iterator = RendererIterator(&renderer, *renderer_n_points_);
//set the RendererIterator parameters
renderer_iterator.angle_step_ = *renderer_angle_step_;
renderer_iterator.radius_min_ = float(*renderer_radius_min_);
renderer_iterator.radius_max_ = float(*renderer_radius_max_);
renderer_iterator.radius_step_ = float(*renderer_radius_step_);
cv::Mat image, depth, mask;
cv::Matx33d R;
cv::Vec3d T;
cv::Matx33f K;
for (size_t i = 0; !renderer_iterator.isDone(); ++i, ++renderer_iterator)
{
std::stringstream status;
status << "Loading images " << (i+1) << "/"
<< renderer_iterator.n_templates();
std::cout << status.str();
cv::Rect rect;
renderer_iterator.render(image, depth, mask, rect);
R = renderer_iterator.R_obj();
T = renderer_iterator.T();
float distance = fabs(renderer_iterator.D_obj() - float(depth.at<ushort>(depth.rows/2.0f, depth.cols/2.0f)/1000.0f));
K = cv::Matx33f(float(*renderer_focal_length_x_), 0.0f, float(rect.width)/2.0f, 0.0f, float(*renderer_focal_length_y_), float(rect.height)/2.0f, 0.0f, 0.0f, 1.0f);
std::vector<cv::Mat> sources(2);
sources[0] = image;
sources[1] = depth;
#if LINEMOD_VIZ_IMG
// Display the rendered image
if (*visualize_)
{
cv::namedWindow("Rendering");
if (!image.empty()) {
cv::imshow("Rendering", image);
cv::waitKey(1);
}
}
#endif
int template_in = detector_->addTemplate(sources, "object1", mask);
if (template_in == -1)
{
// Delete the status
for (size_t j = 0; j < status.str().size(); ++j)
std::cout << '\b';
continue;
}
// Also store the pose of each template
Rs_->push_back(cv::Mat(R));
Ts_->push_back(cv::Mat(T));
distances_->push_back(distance);
Ks_->push_back(cv::Mat(K));
// Delete the status
for (size_t j = 0; j < status.str().size(); ++j)
std::cout << '\b';
}
return ecto::OK;
}
