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; }